Diff: main.cpp

Revision:

0:06f2d82c048c

Child:

1:4fb26a2daf03

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 13 22:48:12 2017 +0000
@@ -0,0 +1,95 @@
+#include "mbed.h"
+#include "Servo.h"
+union floatInByte
+{
+    unsigned short         sd;
+    unsigned char c[2];
+};
+
+Serial master(dp16,dp15);
+Servo arm[4] = {Servo(dp1),Servo(dp2),Servo(dp24),Servo(dp18)};
+floatInByte data[4][2] = {{0,0},{0,0},{0,0},{0,0}};
+
+char RXDATA[24] = {0,0,0,0,255,255,155,255,5,255,000,255,
+                   0,0,0,0,255,255,155,225,5,205,000,55};
+
+void run(int i);
+void rx(){
+    if(master.getc() == 'H'){
+        for(int i = 1;i < 24;i++){
+            RXDATA[i] = master.getc();
+        }
+        for(int i  = 0 ;  i < 4 ; i++)run(i);
+    }
+}
+void waitTime(float ti){
+    Timer t;
+    t.start();
+    while(ti > t.read());
+    t.stop();
+    return;
+}
+#if 1//R
+#define head 4
+    void run(int i){
+        //for(int i = 0 ; i < 4 ; i ++ ){
+        for(int j = 0 ; j < 2; j++){
+            data[i][0].c[j] = RXDATA[head + i*2 + j];
+            
+        }  
+        arm[i] = (float)data[i][0].sd/0xffff;
+        //printf("%d %f  ",i,float(data[i].sd)/0xffff);
+        //}
+        //printf("\n");
+    }
+#else//L
+#define head 4+12
+    void run(int i){
+        //for(int i = 0 ; i < 4 ; i ++ ){
+        for(int j = 0 ; j < 2; j++){
+            data[i][0].c[j] = RXDATA[head + i*2 + j];
+        } 
+        arm[i] = (float)data[i][0].sd/0xffff;
+        //printf("%d %f %d %d ",i,float(data[i].sd)/0xffff , data[i].c[1] , data[i].c[0]);
+        //}
+        //printf("\n");
+    }
+#endif
+
+double armSampleTime[4] = {0,0,0,0};
+bool armFlag[4] = {false,false,false,false};
+void update(int i){
+        //armSampleTime[i] += 10.0/1000.0;
+        double r = (float)data[i][0].sd/0xffff;
+        double a = (float)data[i][1].sd/0xffff;
+        double c = r + (a-r)*exp(-1.0/(10.0/1000.0)*armSampleTime[i]);
+        printf("%5f %5f",r,a);arm[i] = r;
+        if(fabs(r-c) < 0.001F)
+        {
+            data[i][1] = data[i][0];
+            armSampleTime[i] = 0;
+            armFlag[i] = true;
+        }
+        printf("%5f ",c);//arm[i] = c;
+}
+double armOffset[4] = {0,0,0,0};
+int main() {
+    master.baud(115200);
+    master.attach(&rx);
+    //printf("start ");
+    //pc.baud(115200);
+    while(1) {
+        //pc.printf("%x ",master.getc());
+        //for(int i = 0 ; i<100 ; i++){
+        //
+        /*
+        for(int i = 0 ; i < 4;i++){
+            update(i);
+            run(i);
+            
+        }
+        waitTime(0.01);
+        */
+        wait(0.001);
+    }
+}
\ No newline at end of file