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 Serial master(dp16,dp15);
00009 Servo arm[4] = {Servo(dp1),Servo(dp2),Servo(dp24),Servo(dp18)};
00010 floatInByte data[4][2] = {{0,0},{0,0},{0,0},{0,0}};
00011 
00012 char RXDATA[24] = {0,0,0,0,255,255,155,255,5,255,000,255,
00013                    0,0,0,0,255,255,155,225,5,205,000,55};
00014 
00015 void run(int i);
00016 void rx(){
00017     if(master.getc() == 'H'){
00018         for(int i = 1;i < 24;i++){
00019             RXDATA[i] = master.getc();
00020         }
00021     }
00022 }
00023 void waitTime(float ti){
00024     Timer t;
00025     t.start();
00026     while(ti > t.read());
00027     t.stop();
00028     return;
00029 }
00030 #if 0//R
00031 #define head 4
00032     void run(int i){
00033         //for(int i = 0 ; i < 4 ; i ++ ){
00034         for(int j = 0 ; j < 2; j++){
00035             data[i][0].c[j] = RXDATA[head + i*2 + j];   
00036         }
00037         arm[i] = (float)data[i][0].sd/0xffff;
00038         //printf("%d %f  ",i,float(data[i].sd)/0xffff);
00039         //}
00040         //printf("\n");
00041     }
00042 #else//L
00043 #define head 4+12
00044     void run(int i){
00045         //for(int i = 0 ; i < 4 ; i ++ ){
00046         for(int j = 0 ; j < 2; j++){
00047             data[i][0].c[j] = RXDATA[head + i*2 + j];
00048         }
00049         arm[i] = (float)data[i][0].sd/0xffff;
00050         //printf("%d %f %d %d ",i,float(data[i].sd)/0xffff , data[i].c[1] , data[i].c[0]);
00051         //}
00052         //printf("\n");
00053     }
00054 #endif
00055 
00056 int main() {
00057     master.baud(115200);
00058     master.attach(&rx);
00059     //printf("start ");
00060     //pc.baud(115200);
00061     for(int i = 0 ; i < 4 ;i++){
00062         arm[i].calibrate(0.0008,270.0/2);
00063     }
00064     while(1) {
00065         for(int i  = 0 ;  i < 4 ; i++)run(i);
00066         waitTime(20.0/1000.0);
00067     }
00068 }