Diff: main.cpp

Revision:

14:c51c4e0f3bc9

Parent:

13:698bd4df9702

Child:

15:d058877eb501

--- a/main.cpp	Fri Feb 08 15:23:40 2019 +0000
+++ b/main.cpp	Fri Feb 08 19:24:15 2019 +0000
@@ -1,7 +1,8 @@
 #include "mbed.h"
 #include "communication_1.h"
 #include "MX106.h"
-
+#include "AX12.h"
+#define SPEED 100
 // Utility
 InterruptIn button(USER_BUTTON);
 DigitalOut led(LED1);
@@ -12,6 +13,7 @@
 MX106 motor_1(wire, 1, 1);
 MX106 motor_2(wire, 2, 1);
 MX106 motor_3(wire, 3, 1);
+AX12  motor_4(wire, 4, 1);
 
 void button_int_handler()
 {
@@ -27,31 +29,111 @@
 CANMessage messageOut;
 
 int filter = can1.filter(0x000, 0x400, CANStandard);
-
+int pose;
 void canrx()
 {
   while(1)
   {
     if(can1.read(messageIn, filter))
     {
-      printf("CAN: mess %d\n\r", (messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)));
-      printf("CANaacc: id %x \n\r ",messageIn.id);
+      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)
       {
-          
+            if (pose==0)
+            {
+                motor_1.setSpeed(-SPEED);
+                printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
+            }
+            
+            else if (pose==50)
+            {
+                motor_1.setSpeed(0);
+                printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
+            }
+            else if (pose==100)
+            {
+               
+                motor_1.setSpeed(0);
+                printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition());
+                
+            }
+            else  
+                motor_1.setSpeed(0);        
       }
       else if((messageIn.id & 0x0FF) == 0x50)
       {
-          
-      }
+            if (pose==0)
+            {
+                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 (pose==50)
+            {
+                motor_2.setSpeed(0);
+                printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
+            }
+            else if (pose==100)
+            {
+               
+                motor_2.setSpeed(0);
+                printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition());
+                if (motor_2.getPosition()>90) motor_2.setSpeed(0);
+            }
+            else 
+                motor_1.setSpeed(0); 
+       }
       else if((messageIn.id & 0x0FF) == 0x60)
       {
-          
+         
+            if (pose==0)
+            {
+                motor_3.setSpeed(-SPEED);
+                printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
+            }
+            
+            else if (pose==50)
+            {
+                motor_3.setSpeed(0);
+                printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
+            }
+            else if (pose==100)
+            {
+               
+                motor_3.setSpeed(0);
+                printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition());
+                
+            }
+            else 
+                motor_3.setSpeed(0); 
       }
       else if((messageIn.id & 0x0FF) == 0x70)
       {
           
+            if (pose==0)
+            {
+                motor_4.setSpeed(-SPEED);
+                printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
+            }
+            
+            else if (pose==50)
+            {
+                motor_1.setSpeed(0);
+                printf("Dynamixel 4 Position : %f \n\r ", motor_1.getPosition());
+            }
+            else if (pose==100)
+            {
+               
+                motor_1.setSpeed(0);
+                printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
+                
+            }
+            else 
+                motor_4.setSpeed(0); 
       }
     }
   }
@@ -66,23 +148,32 @@
    
    // Setup Motor1 MultiTurn
    motor_1.setMotorEnabled(1);
-   motor_1.setMode(2);
-   motor_1.setSpeed(90);
+   motor_1.setMode(0);
+   motor_1.setSpeed(0);
+   printf("Dynamixel 1 Position init: %f \n\r ", motor_1.getPosition());
    //motor_1.setGoalPosition(0);
    wait(10);
    
    // Setup Motor2 MultiTurn
    motor_2.setMotorEnabled(1);
-   motor_2.setMode(2);
-   motor_2.setSpeed(90);
+   motor_2.setMode(0);
+   motor_2.setSpeed(0);
+   printf("Dynamixel 2 Position init: %f \n\r ", motor_2.getPosition());
    //motor_2.setGoalPosition(0);
    wait(10);
    
    // Setup Motor3 MultiTurn
    motor_3.setMotorEnabled(1);
-   motor_3.setMode(2);
-   motor_3.setSpeed(90);
+   motor_3.setMode(0);
+   motor_3.setSpeed(0);
+   printf("Dynamixel 3 Position init: %f \n\r ", motor_3.getPosition());
+   wait(10);
    //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(10);
 
    printf("DYNAMIXEL: Init DONE\n\r");