xLAB - acutators / Mbed 2 deprecated mrf24jclient_vest1

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?

UserRevisionLine numberNew 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 }