Diff: main.cpp

Revision:

48:96b5f5ebdfb0

Parent:

47:6ea046767494

Child:

49:b041c815c063

--- a/main.cpp	Tue Sep 12 10:36:31 2017 +0000
+++ b/main.cpp	Wed Sep 13 00:19:44 2017 +0000
@@ -14,7 +14,6 @@
 #include "cyclic_IO.h"
 #include "cylinder.h"
 
-
 #define pc_baud     460800
 #define sbdbt_tx    p13
 #define sbdbt_rx    p14
@@ -59,9 +58,9 @@
 //#define pin_interrupt_reload p30 //
 
 DigitalOut led1(LED1);
-//DigitalOut led2(LED2);
-//DigitalOut led3(LED3);
-//DigitalOut led4(LED4);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
 DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min);
 Serial pc(USBTX,USBRX);
 RS422 rs422(rs422_tx, rs422_rx);
@@ -129,10 +128,10 @@
 {
     setup();
     while(1) {
-//        led1 = interrupt_sholderleft_max;
-//        led2 = interrupt_sholderleft_min;
-//        led3 = interrupt_sholderright_max;
-//        led4 = interrupt_sholderright_min;
+        led1 = interrupt_sholderleft_max;
+        led2 = interrupt_sholderleft_min;
+        led3 = interrupt_sholderright_max;
+        led4 = interrupt_sholderright_min;
         //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg());
         //pc.printf("rise state : %d\r\n",cyclic_servo.getState());
         pc.printf("Sbdbtstate : %d\r\n",sbdbt.get_pairingState());
@@ -194,9 +193,10 @@
 //cylinder
 void cylinder_origin_first()
 {
-    while(interrupt_cylinder_min == 1) {
+    while(interrupt_cylinder_min == 1){
         rs422.put(5, -0.4, 0.0);
     }
+    rs422.put(5, 0.0, 0.0);
     enc_cylinder.origin();
 }
 
@@ -300,7 +300,7 @@
     cylinder_origin();
     //boost();
 
-    led1 = sbdbt.get_pairingState();
+    //led1 = sbdbt.get_pairingState();
     sbdbt_indigator = sbdbt.get_pairingState();
 
     if(sbdbt.up) {
@@ -329,7 +329,7 @@
                 counter ++;
                 break;
             case 3:
-                rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_left_cal()*sholder_power);
+                rs422.put(id[counter], -1*shoulder_right_cal()*sholder_power,shoulder_left_cal()*sholder_power);
                 counter ++;
                 break;
             case 4:
@@ -338,10 +338,31 @@
                 break;
             default:
                 break;
-        };
+        }
     }else{
-        for(int count_i = 0;count_i<=nucleo_num;count_i++){
-            rs422.put(id[count_i],0.0,0.0);
+        switch (counter) {
+            case 0:
+                rs422.put(id[counter],0.0,0.0);
+                counter++;
+                break;
+            case 1:
+                rs422.put(id[counter],0.0,0.0);
+                counter ++;
+                break;
+            case 2:
+                rs422.put(id[counter],0.0,0.0);
+                counter ++;
+                break;
+            case 3:
+                rs422.put(id[counter],0.0,0.0);
+                counter ++;
+                break;
+            case 4:
+                rs422.put(id[counter],0.0,0.0);
+                counter = 0;
+                break;
+            default:
+                break;
         }
     }
 }
\ No newline at end of file