Diff: main.cpp

Revision:

18:841ab8c1bdbd

Parent:

17:2ab8feddb7c8

Child:

19:7f8c174448d0

--- a/main.cpp	Sat Feb 09 16:53:04 2019 +0000
+++ b/main.cpp	Sat Apr 06 17:00:28 2019 +0000
@@ -3,20 +3,24 @@
 #include "MX106.h"
 #include "AX12.h"
 #include "Servo.h"
+
 #define SPEED 100
+
 // Utility
 InterruptIn button(USER_BUTTON);
 DigitalOut led(LED1);
 
 // Motor Control
-//Serial pc(USBTX, USBRX);
 communication_1 wire(PA_9, PA_10, 57600);
 MX106 motor_1(wire, 1, 1);
 MX106 motor_2(wire, 2, 1);
 MX106 motor_3(wire, 3, 1);
 AX12  motor_4(wire, 4, 1);
+
+// Camera PanTilt Control
 Servo cam1 (D9);
 Servo cam2 (D10);
+
 void button_int_handler()
 {
 
@@ -24,15 +28,16 @@
 
 // CAN
 //Thread canrxa;
-//Mutex mutex;
 CAN can1(PA_11, PA_12);     // RX, TX
 
 CANMessage messageIn;
 CANMessage messageOut;
 
 int filter = can1.filter(0x000, 0x400, CANStandard);
-int pose=1;
+
+int pose;
 int current_pose[6];
+
 void canrx()
 {
     if(can1.read(messageIn, filter))
@@ -137,20 +142,19 @@
   
    while(true)
    {
-      
             canrx();
-            if (current_pose[0]==0)
+            if (current_pose[0]==1)
             {
                 motor_1.setSpeed(-SPEED);
                 printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
             }
             
-            else if (current_pose[0]==50)
+            else if (current_pose[0]==0)
             {
                 motor_1.setSpeed(0);
                 printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
             }
-            else if (current_pose[0]==100)
+            else if (current_pose[0]==2)
             {
                
                 motor_1.setSpeed(SPEED);
@@ -158,19 +162,19 @@
                 
             }
             
-            else if (current_pose[1]==0)
+            if (current_pose[1]==1)
             {
                 motor_2.setSpeed(-SPEED);
                 printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
                 if (motor_2.getPosition()<-90) motor_2.setSpeed(0);
             }
             
-            else if (current_pose[1]==50)
+            else if (current_pose[1]==0)
             {
                 motor_2.setSpeed(0);
                 printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
             }
-            else if (current_pose[1]==100)
+            else if (current_pose[1]==2)
             {
                
                 motor_2.setSpeed(SPEED);
@@ -178,18 +182,18 @@
                 if (motor_2.getPosition()>90) motor_2.setSpeed(0);
             }
             
-             else if (current_pose[2]==0)
+            if (current_pose[2]==1)
             {
                 motor_3.setSpeed(-SPEED);
                 printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
             }
             
-            else if (current_pose[2]==50)
+            else if (current_pose[2]==0)
             {
                 motor_3.setSpeed(0);
                 printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
             }
-            else if (current_pose[2]==100)
+            else if (current_pose[2]==2)
             {
                
                 motor_3.setSpeed(SPEED);
@@ -197,25 +201,25 @@
                 
             }
             
-            else if (current_pose[3]==0)
+            if (current_pose[3]==1)
             {
                 motor_4.setSpeed(-SPEED);
                 printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
             }
             
-            else if (current_pose[3]==50)
+            else if (current_pose[3]==0)
             {
                 motor_4.setSpeed(0);
                 printf("Dynamixel 4 Position : %f \n\r ", motor_1.getPosition());
             }
-            else if (current_pose[3]==100)
+            else if (current_pose[3]==2)
             {
                
                 motor_4.setSpeed(SPEED);
                 printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());    
             }
             
-            else if (current_pose[4]==0)
+            if (current_pose[4]==1)
             {
                 if (cam1==0)
                     cam1=cam1;
@@ -224,7 +228,7 @@
                 wait(0.2);
                 
             }
-            else if (current_pose[4]==100)
+            else if (current_pose[4]==2)
             {
             
                 if (cam1==1)
@@ -233,7 +237,8 @@
                 
                 wait(0.2);
             }
-             else if (current_pose[5]==0)
+            
+            if (current_pose[5]==1)
             {
                 if (cam2==0)
                     cam1=cam1;
@@ -242,7 +247,7 @@
                 wait(0.2);
                 
             }
-            else if (current_pose[5]==100)
+            else if (current_pose[5]==2)
             {
             
                 if (cam2==1)
@@ -250,8 +255,6 @@
                 else cam2 = cam2+0.2;
                 
                 wait(0.2);
-            }
-             
-                        
+            }           
    }
 }