main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 union floatInByte
00004 {
00005     unsigned short         sd;
00006     unsigned char c[2];
00007 };
00008 
00009 Serial master(dp16,dp15);
00010 Servo arm[4] = {Servo(dp1),Servo(dp2),Servo(dp24),Servo(dp18)};
00011 floatInByte data[4][2] = {{0,0},{0,0},{0,0},{0,0}};
00012 
00013 char RXDATA[24] = {0,0,0,0,255,255,155,255,5,255,000,255,
00014                    0,0,0,0,255,255,155,225,5,205,000,55};
00015 
00016 void run(int i);
00017 void rx(){
00018     if(master.getc() == 'H'){
00019         for(int i = 1;i < 24;i++){
00020             RXDATA[i] = master.getc();
00021         }
00022     }
00023 }
00024 void waitTime(float ti){
00025     Timer t;
00026     t.start();
00027     while(ti > t.read());
00028     t.stop();
00029     return;
00030 }
00031 #if 0//R
00032 #define head 4
00033     void run(int i){
00034         //for(int i = 0 ; i < 4 ; i ++ ){
00035         for(int j = 0 ; j < 2; j++){
00036             data[i][0].c[j] = RXDATA[head + i*2 + j];   
00037         }
00038         arm[i] = (float)data[i][0].sd/0xffff;
00039         //printf("%d %f  ",i,float(data[i].sd)/0xffff);
00040         //}
00041         //printf("\n");
00042     }
00043 #else//L
00044 #define head 4+12
00045     void run(int i){
00046         //for(int i = 0 ; i < 4 ; i ++ ){
00047         for(int j = 0 ; j < 2; j++){
00048             data[i][0].c[j] = RXDATA[head + i*2 + j];
00049         }
00050         arm[i] = (float)data[i][0].sd/0xffff;
00051         //printf("%d %f %d %d ",i,float(data[i].sd)/0xffff , data[i].c[1] , data[i].c[0]);
00052         //}
00053         //printf("\n");
00054     }
00055 #endif
00056 
00057 double armSampleTime[4] = {0,0,0,0};
00058 bool armFlag[4] = {false,false,false,false};
00059 void update(int i){
00060         //armSampleTime[i] += 10.0/1000.0;
00061         double r = (float)data[i][0].sd/0xffff;
00062         double a = (float)data[i][1].sd/0xffff;
00063         double c = r + (a-r)*exp(-1.0/(10.0/1000.0)*armSampleTime[i]);
00064         printf("%5f %5f",r,a);arm[i] = r;
00065         if(fabs(r-c) < 0.001F)
00066         {
00067             data[i][1] = data[i][0];
00068             armSampleTime[i] = 0;
00069             armFlag[i] = true;
00070         }
00071         printf("%5f ",c);//arm[i] = c;
00072 }
00073 double armOffset[4] = {0,0,0,0};
00074 int main() {
00075     master.baud(115200);
00076     master.attach(&rx);
00077     //printf("start ");
00078     //pc.baud(115200);
00079     while(1) {
00080         //pc.printf("%x ",master.getc());
00081         //for(int i = 0 ; i<100 ; i++){
00082         //
00083         /*
00084         for(int i = 0 ; i < 4;i++){
00085             update(i);
00086             run(i);
00087             
00088         }
00089         waitTime(0.01);
00090         */
00091         for(int i  = 0 ;  i < 4 ; i++)run(i);
00092         waitTime(20.0/1000.0);
00093     }
00094 }