ese519
Dependencies: MRF24J40 mbed-rtos mbed
main.cpp@0:ef293aec3a85, 2015-03-21 (annotated)
- Committer:
- Jing_Qiu
- Date:
- Sat Mar 21 02:52:43 2015 +0000
- Revision:
- 0:ef293aec3a85
ese519
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jing_Qiu | 0:ef293aec3a85 | 1 | #include "mbed.h" |
Jing_Qiu | 0:ef293aec3a85 | 2 | #include "MRF24J40.h" |
Jing_Qiu | 0:ef293aec3a85 | 3 | #include "Actuator.h" |
Jing_Qiu | 0:ef293aec3a85 | 4 | #include <string> |
Jing_Qiu | 0:ef293aec3a85 | 5 | #include <sstream> |
Jing_Qiu | 0:ef293aec3a85 | 6 | #include <vector> |
Jing_Qiu | 0:ef293aec3a85 | 7 | #include <stdio.h> /* printf, scanf, puts, NULL */ |
Jing_Qiu | 0:ef293aec3a85 | 8 | #include <stdlib.h> /* srand, rand */ |
Jing_Qiu | 0:ef293aec3a85 | 9 | #include <time.h> |
Jing_Qiu | 0:ef293aec3a85 | 10 | #include "rtos.h" |
Jing_Qiu | 0:ef293aec3a85 | 11 | |
Jing_Qiu | 0:ef293aec3a85 | 12 | DigitalOut myled(LED3); //for vest test |
Jing_Qiu | 0:ef293aec3a85 | 13 | DigitalOut led1(LED1); //for RF test |
Jing_Qiu | 0:ef293aec3a85 | 14 | |
Jing_Qiu | 0:ef293aec3a85 | 15 | |
Jing_Qiu | 0:ef293aec3a85 | 16 | PwmOut motor1(p25); |
Jing_Qiu | 0:ef293aec3a85 | 17 | PwmOut motor2(p26); |
Jing_Qiu | 0:ef293aec3a85 | 18 | PwmOut motor3(p23); |
Jing_Qiu | 0:ef293aec3a85 | 19 | PwmOut motor4(p24); |
Jing_Qiu | 0:ef293aec3a85 | 20 | PwmOut motor5(p22); |
Jing_Qiu | 0:ef293aec3a85 | 21 | PwmOut motor6(p21); |
Jing_Qiu | 0:ef293aec3a85 | 22 | Serial pc(USBTX, USBRX); |
Jing_Qiu | 0:ef293aec3a85 | 23 | Timeout to1; |
Jing_Qiu | 0:ef293aec3a85 | 24 | Timeout to2; |
Jing_Qiu | 0:ef293aec3a85 | 25 | Timeout to3; |
Jing_Qiu | 0:ef293aec3a85 | 26 | Timeout to4; |
Jing_Qiu | 0:ef293aec3a85 | 27 | Timeout to5; |
Jing_Qiu | 0:ef293aec3a85 | 28 | Timeout to6; |
Jing_Qiu | 0:ef293aec3a85 | 29 | |
Jing_Qiu | 0:ef293aec3a85 | 30 | Actuator actuator1(0,1,motor1,to1); |
Jing_Qiu | 0:ef293aec3a85 | 31 | Actuator actuator2(0,2,motor2,to2); |
Jing_Qiu | 0:ef293aec3a85 | 32 | Actuator actuator3(0,3,motor3,to3); |
Jing_Qiu | 0:ef293aec3a85 | 33 | Actuator actuator4(0,4,motor4,to4); |
Jing_Qiu | 0:ef293aec3a85 | 34 | Actuator actuator5(0,5,motor5,to5); |
Jing_Qiu | 0:ef293aec3a85 | 35 | Actuator actuator6(0,5,motor6,to6); |
Jing_Qiu | 0:ef293aec3a85 | 36 | |
Jing_Qiu | 0:ef293aec3a85 | 37 | |
Jing_Qiu | 0:ef293aec3a85 | 38 | |
Jing_Qiu | 0:ef293aec3a85 | 39 | // Timeout for vibration |
Jing_Qiu | 0:ef293aec3a85 | 40 | |
Jing_Qiu | 0:ef293aec3a85 | 41 | // RF tranceiver to link with handheld. |
Jing_Qiu | 0:ef293aec3a85 | 42 | MRF24J40 mrf(p11, p12, p13, p14, p21); |
Jing_Qiu | 0:ef293aec3a85 | 43 | |
Jing_Qiu | 0:ef293aec3a85 | 44 | char rxBuffer[128]; |
Jing_Qiu | 0:ef293aec3a85 | 45 | uint8_t rxLen; |
Jing_Qiu | 0:ef293aec3a85 | 46 | uint32_t duration; |
Jing_Qiu | 0:ef293aec3a85 | 47 | uint32_t viberation; |
Jing_Qiu | 0:ef293aec3a85 | 48 | |
Jing_Qiu | 0:ef293aec3a85 | 49 | int rf_receive(char *data, uint8_t maxLength) |
Jing_Qiu | 0:ef293aec3a85 | 50 | { |
Jing_Qiu | 0:ef293aec3a85 | 51 | uint8_t len = mrf.Receive((uint8_t *)data, maxLength); |
Jing_Qiu | 0:ef293aec3a85 | 52 | //pc.printf("data: %s, %d\r\n",data,len); |
Jing_Qiu | 0:ef293aec3a85 | 53 | uint8_t header[8]= {1, 8, 0, 0xA1, 0xB2, 0xC3, 0xD4, 0x00}; |
Jing_Qiu | 0:ef293aec3a85 | 54 | if(len > 10) { |
Jing_Qiu | 0:ef293aec3a85 | 55 | //Remove the header and footer of the message |
Jing_Qiu | 0:ef293aec3a85 | 56 | for(uint8_t i = 0; i < len-2; i++) { |
Jing_Qiu | 0:ef293aec3a85 | 57 | if(i<8) { |
Jing_Qiu | 0:ef293aec3a85 | 58 | //Make sure our header is valid first |
Jing_Qiu | 0:ef293aec3a85 | 59 | if(data[i] != header[i]) |
Jing_Qiu | 0:ef293aec3a85 | 60 | return 0; |
Jing_Qiu | 0:ef293aec3a85 | 61 | } else { |
Jing_Qiu | 0:ef293aec3a85 | 62 | data[i-8] = data[i]; |
Jing_Qiu | 0:ef293aec3a85 | 63 | } |
Jing_Qiu | 0:ef293aec3a85 | 64 | } |
Jing_Qiu | 0:ef293aec3a85 | 65 | pc.printf("Received: %s length:%d\r\n", data, ((int)len)-10); |
Jing_Qiu | 0:ef293aec3a85 | 66 | } |
Jing_Qiu | 0:ef293aec3a85 | 67 | return ((int)len)-10; |
Jing_Qiu | 0:ef293aec3a85 | 68 | } |
Jing_Qiu | 0:ef293aec3a85 | 69 | |
Jing_Qiu | 0:ef293aec3a85 | 70 | std::vector<std::string> split(const std::string &str, char delimiter, std::vector<std::string> extract) |
Jing_Qiu | 0:ef293aec3a85 | 71 | { |
Jing_Qiu | 0:ef293aec3a85 | 72 | std::stringstream ss(str); |
Jing_Qiu | 0:ef293aec3a85 | 73 | std::string item; |
Jing_Qiu | 0:ef293aec3a85 | 74 | while (std::getline(ss, item, delimiter)) { |
Jing_Qiu | 0:ef293aec3a85 | 75 | extract.push_back(item); |
Jing_Qiu | 0:ef293aec3a85 | 76 | } |
Jing_Qiu | 0:ef293aec3a85 | 77 | return extract; |
Jing_Qiu | 0:ef293aec3a85 | 78 | } |
Jing_Qiu | 0:ef293aec3a85 | 79 | |
Jing_Qiu | 0:ef293aec3a85 | 80 | |
Jing_Qiu | 0:ef293aec3a85 | 81 | void updateMotors(uint32_t dur) { |
Jing_Qiu | 0:ef293aec3a85 | 82 | float durinsec = dur/1000.0; |
Jing_Qiu | 0:ef293aec3a85 | 83 | if(actuator1.getM()) actuator1.getTo().attach(&actuator1,&Actuator::shot,durinsec); |
Jing_Qiu | 0:ef293aec3a85 | 84 | if(actuator2.getM()) actuator2.getTo().attach(&actuator2,&Actuator::shot,durinsec); |
Jing_Qiu | 0:ef293aec3a85 | 85 | if(actuator3.getM()) actuator3.getTo().attach(&actuator3,&Actuator::shot,durinsec); |
Jing_Qiu | 0:ef293aec3a85 | 86 | if(actuator4.getM()) actuator4.getTo().attach(&actuator4,&Actuator::shot,durinsec); |
Jing_Qiu | 0:ef293aec3a85 | 87 | if(actuator5.getM()) actuator5.getTo().attach(&actuator5,&Actuator::shot,durinsec); |
Jing_Qiu | 0:ef293aec3a85 | 88 | if(actuator6.getM()) actuator6.getTo().attach(&actuator6,&Actuator::shot,durinsec); |
Jing_Qiu | 0:ef293aec3a85 | 89 | } |
Jing_Qiu | 0:ef293aec3a85 | 90 | |
Jing_Qiu | 0:ef293aec3a85 | 91 | |
Jing_Qiu | 0:ef293aec3a85 | 92 | int main() { |
Jing_Qiu | 0:ef293aec3a85 | 93 | myled = 0; |
Jing_Qiu | 0:ef293aec3a85 | 94 | pc.baud(115200); |
Jing_Qiu | 0:ef293aec3a85 | 95 | |
Jing_Qiu | 0:ef293aec3a85 | 96 | pc.printf("Client Vest\r\n"); |
Jing_Qiu | 0:ef293aec3a85 | 97 | while(1){ |
Jing_Qiu | 0:ef293aec3a85 | 98 | // Check if any data was received. |
Jing_Qiu | 0:ef293aec3a85 | 99 | rxLen = rf_receive(rxBuffer,64); |
Jing_Qiu | 0:ef293aec3a85 | 100 | if(rxLen) |
Jing_Qiu | 0:ef293aec3a85 | 101 | { |
Jing_Qiu | 0:ef293aec3a85 | 102 | pc.printf("Received string %s \r\n",rxBuffer); |
Jing_Qiu | 0:ef293aec3a85 | 103 | std::vector<std::string> msg; |
Jing_Qiu | 0:ef293aec3a85 | 104 | /*split the buffer based on the delimiter ',' and save into extract*/ |
Jing_Qiu | 0:ef293aec3a85 | 105 | msg = split(std::string(rxBuffer),'/',msg); |
Jing_Qiu | 0:ef293aec3a85 | 106 | |
Jing_Qiu | 0:ef293aec3a85 | 107 | if(msg[0]=="all"||msg[0]=="vest") |
Jing_Qiu | 0:ef293aec3a85 | 108 | { |
Jing_Qiu | 0:ef293aec3a85 | 109 | std::vector<std::string> extract; |
Jing_Qiu | 0:ef293aec3a85 | 110 | extract = split(msg[3],',',extract); //vest is the second part of the msg |
Jing_Qiu | 0:ef293aec3a85 | 111 | |
Jing_Qiu | 0:ef293aec3a85 | 112 | if(!extract[0].compare("V")) { |
Jing_Qiu | 0:ef293aec3a85 | 113 | |
Jing_Qiu | 0:ef293aec3a85 | 114 | if(!extract[1].compare("A")) { |
Jing_Qiu | 0:ef293aec3a85 | 115 | |
Jing_Qiu | 0:ef293aec3a85 | 116 | const char* control = extract[2].c_str(); |
Jing_Qiu | 0:ef293aec3a85 | 117 | duration = std::atoi(extract[3].c_str()); // duration |
Jing_Qiu | 0:ef293aec3a85 | 118 | viberation = std::atoi(extract[4].c_str()); // viberation |
Jing_Qiu | 0:ef293aec3a85 | 119 | |
Jing_Qiu | 0:ef293aec3a85 | 120 | if(strlen(control)==6) |
Jing_Qiu | 0:ef293aec3a85 | 121 | { |
Jing_Qiu | 0:ef293aec3a85 | 122 | if(control[0]=='1'){ |
Jing_Qiu | 0:ef293aec3a85 | 123 | actuator1.setMotor(viberation); |
Jing_Qiu | 0:ef293aec3a85 | 124 | actuator1.setM(1); |
Jing_Qiu | 0:ef293aec3a85 | 125 | //updateMotors(duration); |
Jing_Qiu | 0:ef293aec3a85 | 126 | } |
Jing_Qiu | 0:ef293aec3a85 | 127 | if(control[1]=='1'){ |
Jing_Qiu | 0:ef293aec3a85 | 128 | actuator2.setMotor(viberation); |
Jing_Qiu | 0:ef293aec3a85 | 129 | actuator2.setM(1); |
Jing_Qiu | 0:ef293aec3a85 | 130 | //updateMotors(duration); |
Jing_Qiu | 0:ef293aec3a85 | 131 | } |
Jing_Qiu | 0:ef293aec3a85 | 132 | if(control[2]=='1'){ |
Jing_Qiu | 0:ef293aec3a85 | 133 | actuator3.setMotor(viberation); |
Jing_Qiu | 0:ef293aec3a85 | 134 | actuator3.setM(1); |
Jing_Qiu | 0:ef293aec3a85 | 135 | //updateMotors(duration); |
Jing_Qiu | 0:ef293aec3a85 | 136 | } |
Jing_Qiu | 0:ef293aec3a85 | 137 | if(control[3]=='1'){ |
Jing_Qiu | 0:ef293aec3a85 | 138 | actuator4.setMotor(viberation); |
Jing_Qiu | 0:ef293aec3a85 | 139 | actuator4.setM(1); |
Jing_Qiu | 0:ef293aec3a85 | 140 | updateMotors(duration); |
Jing_Qiu | 0:ef293aec3a85 | 141 | } |
Jing_Qiu | 0:ef293aec3a85 | 142 | if(control[4]=='1'){ |
Jing_Qiu | 0:ef293aec3a85 | 143 | actuator5.setMotor(viberation); |
Jing_Qiu | 0:ef293aec3a85 | 144 | actuator5.setM(1); |
Jing_Qiu | 0:ef293aec3a85 | 145 | //updateMotors(duration); |
Jing_Qiu | 0:ef293aec3a85 | 146 | } |
Jing_Qiu | 0:ef293aec3a85 | 147 | if(control[5]=='1'){ |
Jing_Qiu | 0:ef293aec3a85 | 148 | actuator6.setMotor(viberation); |
Jing_Qiu | 0:ef293aec3a85 | 149 | actuator6.setM(1); |
Jing_Qiu | 0:ef293aec3a85 | 150 | //updateMotors(duration); |
Jing_Qiu | 0:ef293aec3a85 | 151 | } |
Jing_Qiu | 0:ef293aec3a85 | 152 | updateMotors(duration); |
Jing_Qiu | 0:ef293aec3a85 | 153 | } |
Jing_Qiu | 0:ef293aec3a85 | 154 | else pc.printf("error!\r\n"); |
Jing_Qiu | 0:ef293aec3a85 | 155 | |
Jing_Qiu | 0:ef293aec3a85 | 156 | } |
Jing_Qiu | 0:ef293aec3a85 | 157 | |
Jing_Qiu | 0:ef293aec3a85 | 158 | if(!extract[1].compare("C")){ |
Jing_Qiu | 0:ef293aec3a85 | 159 | actuator1.setMotor(0); |
Jing_Qiu | 0:ef293aec3a85 | 160 | actuator1.setM(0); |
Jing_Qiu | 0:ef293aec3a85 | 161 | actuator2.setMotor(0); |
Jing_Qiu | 0:ef293aec3a85 | 162 | actuator2.setM(0); |
Jing_Qiu | 0:ef293aec3a85 | 163 | actuator3.setMotor(0); |
Jing_Qiu | 0:ef293aec3a85 | 164 | actuator3.setM(0); |
Jing_Qiu | 0:ef293aec3a85 | 165 | actuator4.setMotor(0); |
Jing_Qiu | 0:ef293aec3a85 | 166 | actuator4.setM(0); |
Jing_Qiu | 0:ef293aec3a85 | 167 | actuator5.setMotor(0); |
Jing_Qiu | 0:ef293aec3a85 | 168 | actuator5.setM(0); |
Jing_Qiu | 0:ef293aec3a85 | 169 | actuator6.setMotor(0); |
Jing_Qiu | 0:ef293aec3a85 | 170 | actuator6.setM(0); |
Jing_Qiu | 0:ef293aec3a85 | 171 | viberation = 0; |
Jing_Qiu | 0:ef293aec3a85 | 172 | updateMotors(duration); |
Jing_Qiu | 0:ef293aec3a85 | 173 | } |
Jing_Qiu | 0:ef293aec3a85 | 174 | } |
Jing_Qiu | 0:ef293aec3a85 | 175 | else{ |
Jing_Qiu | 0:ef293aec3a85 | 176 | pc.printf("error!\r\n"); |
Jing_Qiu | 0:ef293aec3a85 | 177 | } |
Jing_Qiu | 0:ef293aec3a85 | 178 | |
Jing_Qiu | 0:ef293aec3a85 | 179 | |
Jing_Qiu | 0:ef293aec3a85 | 180 | |
Jing_Qiu | 0:ef293aec3a85 | 181 | } |
Jing_Qiu | 0:ef293aec3a85 | 182 | |
Jing_Qiu | 0:ef293aec3a85 | 183 | } |
Jing_Qiu | 0:ef293aec3a85 | 184 | } |
Jing_Qiu | 0:ef293aec3a85 | 185 | } |