main.cpp

00001 #include "mbed.h"
00002 #include "MRF24J40.h"
00003 #include "Actuator.h"
00004 #include <string>
00005 #include <sstream>
00006 #include <vector>
00007 #include <stdio.h>      /* printf, scanf, puts, NULL */
00008 #include <stdlib.h>     /* srand, rand */
00009 #include <time.h>
00010 #include "rtos.h"
00011 
00012 DigitalOut myled(LED3);  //for vest test
00013 DigitalOut led1(LED1);   //for RF test
00014 
00015 
00016 PwmOut motor1(p25);
00017 PwmOut motor2(p26);
00018 PwmOut motor3(p23);
00019 PwmOut motor4(p24);
00020 PwmOut motor5(p22);
00021 PwmOut motor6(p21);
00022 Serial pc(USBTX, USBRX);
00023 Timeout to1;
00024 Timeout to2;
00025 Timeout to3;
00026 Timeout to4;
00027 Timeout to5;
00028 Timeout to6;
00029 
00030 Actuator actuator1(0,1,motor1,to1);
00031 Actuator actuator2(0,2,motor2,to2);
00032 Actuator actuator3(0,3,motor3,to3);
00033 Actuator actuator4(0,4,motor4,to4);
00034 Actuator actuator5(0,5,motor5,to5);
00035 Actuator actuator6(0,5,motor6,to6);
00036 
00037 
00038 
00039 // Timeout for vibration
00040 
00041 // RF tranceiver to link with handheld.
00042 MRF24J40 mrf(p11, p12, p13, p14, p21);
00043 
00044 char rxBuffer[128];
00045 uint8_t rxLen;
00046 uint32_t duration;
00047 uint32_t viberation;
00048 
00049 int rf_receive(char *data, uint8_t maxLength)
00050 {
00051     uint8_t len = mrf.Receive((uint8_t *)data, maxLength);
00052     //pc.printf("data: %s, %d\r\n",data,len);
00053     uint8_t header[8]= {1, 8, 0, 0xA1, 0xB2, 0xC3, 0xD4, 0x00};
00054     if(len > 10) {
00055         //Remove the header and footer of the message
00056         for(uint8_t i = 0; i < len-2; i++) {
00057             if(i<8) {
00058                 //Make sure our header is valid first
00059                 if(data[i] != header[i])
00060                     return 0;
00061             } else {
00062                 data[i-8] = data[i];
00063             }
00064         }
00065         pc.printf("Received: %s length:%d\r\n", data, ((int)len)-10);
00066     }
00067     return ((int)len)-10;
00068 }
00069 
00070 std::vector<std::string> split(const std::string &str, char delimiter, std::vector<std::string> extract)
00071 {
00072     std::stringstream ss(str);
00073     std::string item;
00074     while (std::getline(ss, item, delimiter)) {
00075         extract.push_back(item);
00076     }
00077     return extract;
00078 }
00079 
00080 
00081 void updateMotors(uint32_t dur) {
00082     float durinsec = dur/1000.0;
00083     if(actuator1.getM()) actuator1.getTo().attach(&actuator1,&Actuator::shot,durinsec);
00084     if(actuator2.getM()) actuator2.getTo().attach(&actuator2,&Actuator::shot,durinsec);
00085     if(actuator3.getM()) actuator3.getTo().attach(&actuator3,&Actuator::shot,durinsec);
00086     if(actuator4.getM()) actuator4.getTo().attach(&actuator4,&Actuator::shot,durinsec);
00087     if(actuator5.getM()) actuator5.getTo().attach(&actuator5,&Actuator::shot,durinsec);
00088     if(actuator6.getM()) actuator6.getTo().attach(&actuator6,&Actuator::shot,durinsec);
00089 }
00090 
00091 
00092 int main() {
00093     myled = 0;
00094     pc.baud(115200);
00095     
00096     pc.printf("Client Vest\r\n");
00097     while(1){
00098         // Check if any data was received. 
00099         rxLen = rf_receive(rxBuffer,64);
00100         if(rxLen) 
00101         {
00102             pc.printf("Received string %s \r\n",rxBuffer);
00103             std::vector<std::string> msg;
00104             /*split the buffer based on the delimiter ',' and save into extract*/
00105             msg = split(std::string(rxBuffer),'/',msg);
00106             
00107             if(msg[0]=="all"||msg[0]=="vest")
00108             { 
00109             std::vector<std::string> extract;
00110             extract = split(msg[3],',',extract);    //vest is the second part of the msg
00111             
00112                 if(!extract[0].compare("V")) {
00113                 
00114                     if(!extract[1].compare("A")) {
00115                 
00116                     const char* control = extract[2].c_str();
00117                     duration = std::atoi(extract[3].c_str()); // duration
00118                     viberation = std::atoi(extract[4].c_str()); // viberation
00119                 
00120                     if(strlen(control)==6)
00121                     {
00122                     if(control[0]=='1'){
00123                        actuator1.setMotor(viberation);
00124                        actuator1.setM(1);
00125                        //updateMotors(duration);
00126                        }
00127                     if(control[1]=='1'){
00128                        actuator2.setMotor(viberation);
00129                        actuator2.setM(1);
00130                        //updateMotors(duration);
00131                        }
00132                     if(control[2]=='1'){
00133                        actuator3.setMotor(viberation);
00134                        actuator3.setM(1);
00135                        //updateMotors(duration);
00136                        }
00137                     if(control[3]=='1'){
00138                        actuator4.setMotor(viberation);
00139                        actuator4.setM(1);
00140                        updateMotors(duration);
00141                        }
00142                     if(control[4]=='1'){
00143                        actuator5.setMotor(viberation);
00144                        actuator5.setM(1);
00145                        //updateMotors(duration);
00146                        }
00147                     if(control[5]=='1'){
00148                        actuator6.setMotor(viberation);
00149                        actuator6.setM(1);
00150                        //updateMotors(duration);
00151                        }
00152                     updateMotors(duration);
00153                     }
00154                     else pc.printf("error!\r\n");                
00155                 
00156                 }
00157                 
00158                 if(!extract[1].compare("C")){
00159                        actuator1.setMotor(0);
00160                        actuator1.setM(0);
00161                        actuator2.setMotor(0);
00162                        actuator2.setM(0);
00163                        actuator3.setMotor(0);
00164                        actuator3.setM(0);
00165                        actuator4.setMotor(0);
00166                        actuator4.setM(0);
00167                        actuator5.setMotor(0);
00168                        actuator5.setM(0);
00169                        actuator6.setMotor(0);
00170                        actuator6.setM(0);
00171                        viberation = 0;
00172                        updateMotors(duration);
00173                        }            
00174                 }
00175                 else{
00176                     pc.printf("error!\r\n");
00177                     }
00178 
00179             
00180         
00181         }
00182           
00183         }
00184     }
00185 }