Diff: main.cpp

Revision:

19:7f8c174448d0

Parent:

18:841ab8c1bdbd

Child:

20:ff2dab77e3e9

--- a/main.cpp	Sat Apr 06 17:00:28 2019 +0000
+++ b/main.cpp	Tue Apr 16 08:59:06 2019 +0000
@@ -4,7 +4,7 @@
 #include "AX12.h"
 #include "Servo.h"
 
-#define SPEED 100
+#define SPEED 50
 
 // Utility
 InterruptIn button(USER_BUTTON);
@@ -36,225 +36,119 @@
 int filter = can1.filter(0x000, 0x400, CANStandard);
 
 int pose;
-int current_pose[6];
+int current_pose[] = {0, 0, 0, 0, 0, 0};
 
-void canrx()
-{
-    if(can1.read(messageIn, filter))
-    {
-      pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
-      printf("CAN: mess %d\n\r", pose);
-      printf("CAN: id %x \n\r ",messageIn.id);
-      
-      if((messageIn.id & 0x0FF) == 0x40)
-      {
-            //mutex.lock();
-            current_pose[0]=pose;
-            //mutex.unlock();
-      }
-      else if((messageIn.id & 0x0FF) == 0x50)
-      {
-           //mutex.lock();
-            current_pose[1]=pose;
-            //mutex.unlock();
-       }
-       
-      else if((messageIn.id & 0x0FF) == 0x60)
-      {
-             //mutex.lock();
-             current_pose[2]=pose;
-             //mutex.unlock();
-      }
-      else if((messageIn.id & 0x0FF) == 0x70)
-      {
-             //mutex.lock();
-             current_pose[3]=pose;
-            //mutex.unlock();
-      }
-        else if((messageIn.id & 0x0FF) == 0x80)
-        {
-            //mutex.lock();
-            current_pose[4]=pose;
-            //mutex.unlock();
-        }
-         else if((messageIn.id & 0x0FF) == 0x90)
-        {
-            //mutex.lock();
-             current_pose[5]=pose;
-             //mutex.unlock();     
-        }
-    }
-  
-}
-   
 int main()
 {
-printf("DYNAMIXEL: Init \n\r");
-led=1; 
-   wire.trigger();
-   wire.trigger();
-   wire.trigger();
-   wire.trigger();
-   wait(5);
-   // Setup Motor1 MultiTurn
-   motor_1.setMotorEnabled(1);
-   motor_1.setMode(0);
-   //motor_1.setSpeed(0);
-   //printf("Dynamixel 1 Position init: %f \n\r ", motor_1.getPosition());
-  
-   wait(5);
-   printf("DYNAMIXEL: Init DONE 1\n\r");
-   // Setup Motor2 MultiTurn
-   motor_2.setMotorEnabled(1);
-   motor_2.setMode(0);
-   //motor_2.setSpeed(0);
-   //printf("Dynamixel 2 Position init: %f \n\r ", motor_2.getPosition());
-   
-   wait(5);
-   printf("DYNAMIXEL: Init DONE 2\n\r");
-   // Setup Motor3 MultiTurn
-   motor_3.setMotorEnabled(1);
-   motor_3.setMode(0);
-   //motor_3.setSpeed(0);
-   //printf("Dynamixel 3 Position init: %f \n\r ", motor_3.getPosition());
-   wait(5);
-   printf("DYNAMIXEL: Init DONE 3\n\r");
-   //motor_3.setGoalPosition(0);
-   // Setup Motor4 MultiTurn
-   //motor_4.setMotorEnabled(1);
-   //motor_4.setMode(0);
-   //motor_4.setSpeed(0);
-   //printf("Dynamixel 4 Position init: %f \n\r ", motor_4.getPosition());
-   wait(5);
+    can1.frequency(125000);
+    printf("CAN: Init DONE\n\r");
+
+    wire.trigger();
+    wire.trigger();
+    wire.trigger();
+    wire.trigger();
+    wait(1);
     
-   printf("DYNAMIXEL: Init DONE\n\r");
-   
-   button.rise(&button_int_handler);
-  cam1=0,5;
-  cam2=0,5;
-   // CAN Initialization  
-   //canrxa.start(canrx);
+    // Setup Motor1 MultiTurn
+    motor_1.setMotorEnabled(1);
+    motor_1.setMode(0);
+    wait(3);
+    printf("DYNAMIXEL: Init DONE 1\n\r");
+
+    // Setup Motor2 MultiTurn
+    motor_2.setMotorEnabled(1);
+    motor_2.setMode(0);
+    wait(3);
+    printf("DYNAMIXEL: Init DONE 2\n\r");
 
-   printf("DONE: CAN Init\n\r");
-  
-  
-   printf("Running!\n\r");
-  
-   while(true)
-   {
-            canrx();
-            if (current_pose[0]==1)
+    // Setup Motor3 MultiTurn
+    motor_3.setMotorEnabled(1);
+    motor_3.setMode(0);
+    wait(3);
+    printf("DYNAMIXEL: Init DONE 3\n\r");
+
+    // Setup Motor4 MultiTurn
+    motor_4.setMotorEnabled(1);
+    motor_4.setMode(0);
+    wait(3);
+    printf("DYNAMIXEL: Init DONE 4\n\r");
+
+    printf("Running!\n\r");
+
+    while(true)
+    {
+        if(can1.read(messageIn, filter))
+        {
+            pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+
+            if(messageIn.id == 0x40 && pose != current_pose[0])
             {
-                motor_1.setSpeed(-SPEED);
-                printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
-            }
-            
-            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]==2)
-            {
-               
-                motor_1.setSpeed(SPEED);
-                printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
+                if(pose == 1)
+                {
+                    motor_1.setSpeed(-SPEED);
+                }
+                else if(pose == 2)
+                {
+                    motor_1.setSpeed(SPEED);
+                }
+                else
+                {
+                    motor_1.setSpeed(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]==0)
-            {
-                motor_2.setSpeed(0);
-                printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
+                current_pose[0] = pose;
             }
-            else if (current_pose[1]==2)
+            else if(messageIn.id == 0x50 && pose != current_pose[1])
             {
-               
-                motor_2.setSpeed(SPEED);
-                printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
-                if (motor_2.getPosition()>90) motor_2.setSpeed(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]==0)
-            {
-                motor_3.setSpeed(0);
-                printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
+                if(pose == 1)
+                {
+                    motor_2.setSpeed(-SPEED);
+                }
+                else if(pose == 2)
+                {
+                    motor_2.setSpeed(SPEED);
+                }
+                else
+                {
+                    motor_2.setSpeed(0);
+                }
+                
+                current_pose[1] = pose;
             }
-            else if (current_pose[2]==2)
-            {
-               
-                motor_3.setSpeed(SPEED);
-                printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
-                
-            }
-            
-            if (current_pose[3]==1)
-            {
-                motor_4.setSpeed(-SPEED);
-                printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
-            }
-            
-            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]==2)
+            else if(messageIn.id == 0x60 && pose != current_pose[2])
             {
-               
-                motor_4.setSpeed(SPEED);
-                printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());    
-            }
-            
-            if (current_pose[4]==1)
-            {
-                if (cam1==0)
-                    cam1=cam1;
-                else cam1 = cam1-0.2;
-                
-                wait(0.2);
-                
-            }
-            else if (current_pose[4]==2)
-            {
-            
-                if (cam1==1)
-                    cam1=cam1;
-                else cam1 = cam1+0.2;
+                if(pose == 1)
+                {
+                    motor_3.setSpeed(-SPEED);
+                }
+                else if(pose == 2)
+                {
+                    motor_3.setSpeed(SPEED);
+                }
+                else
+                {
+                    motor_3.setSpeed(0);
+                }
                 
-                wait(0.2);
-            }
-            
-            if (current_pose[5]==1)
-            {
-                if (cam2==0)
-                    cam1=cam1;
-                else cam2 = cam2-0.2;
-                
-                wait(0.2);
-                
+                current_pose[2] = pose;
             }
-            else if (current_pose[5]==2)
+            else if(messageIn.id == 0x70 && pose != current_pose[3])
             {
-            
-                if (cam2==1)
-                    cam2=cam2;
-                else cam2 = cam2+0.2;
+                if(pose == 1)
+                {
+                    motor_4.setSpeed(-SPEED);
+                }
+                else if(pose == 2)
+                {
+                    motor_4.setSpeed(SPEED);
+                }
+                else
+                {
+                    motor_4.setSpeed(0);
+                }
                 
-                wait(0.2);
-            }           
-   }
-}
+                current_pose[3] = pose;
+            }
+        }
+    }
+}
\ No newline at end of file