Diff: main.cpp

Revision:

15:c1a790a1e999

Parent:

13:698bd4df9702

diff -r a3c4c15b8e6b -r c1a790a1e999 main.cpp
--- a/main.cpp	Wed Mar 27 08:25:05 2019 +0000
+++ b/main.cpp	Tue Apr 16 08:58:02 2019 +0000
@@ -2,13 +2,14 @@
 #include "communication_1.h"
 #include "MX106.h"
 
+#define SPEED 50
+
 // Utility
 InterruptIn button(USER_BUTTON);
 DigitalOut led(LED1);
 
 // Motor Control
-Serial pc(USBTX, USBRX);
-communication_1 wire(PA_9, PA_10, 200000);
+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);
@@ -22,36 +23,34 @@
 Thread canrxa;
 
 CAN can1(PA_11, PA_12);     // RX, TX
-
 CANMessage messageIn;
 CANMessage messageOut;
 
 int filter = can1.filter(0x000, 0x400, CANStandard);
 
+int pose;
+int current_pose[] = {0, 0, 0};
+int desired_pose[] = {0, 0, 0};
+
 void canrx()
 {
-  while(1)
+  //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);
       
       if((messageIn.id & 0x0FF) == 0x40)
       {
-          
+          desired_pose[0] = pose;
       }
       else if((messageIn.id & 0x0FF) == 0x50)
       {
-          
+          desired_pose[1] = pose;
       }
       else if((messageIn.id & 0x0FF) == 0x60)
       {
-          
-      }
-      else if((messageIn.id & 0x0FF) == 0x70)
-      {
-          
+          desired_pose[2] = pose;
       }
     }
   }
@@ -59,6 +58,10 @@
    
 int main()
 {
+   can1.frequency(125000);
+   
+   printf("DYNAMIXEL: Init START");
+   
    wire.trigger();
    wire.trigger();
    wire.trigger();
@@ -67,38 +70,70 @@
    // Setup Motor1 MultiTurn
    motor_1.setMotorEnabled(1);
    motor_1.setMode(2);
-   motor_1.setSpeed(90);
-   //motor_1.setGoalPosition(0);
-   wait(10);
+   wait(1);
+   printf("DYNAMIXEL: Init 1 DONE");
    
    // Setup Motor2 MultiTurn
    motor_2.setMotorEnabled(1);
    motor_2.setMode(2);
-   motor_2.setSpeed(90);
-   //motor_2.setGoalPosition(0);
-   wait(10);
+   wait(1);
+   printf("DYNAMIXEL: Init 2 DONE");
    
    // Setup Motor3 MultiTurn
    motor_3.setMotorEnabled(1);
    motor_3.setMode(2);
-   motor_3.setSpeed(90);
-   //motor_3.setGoalPosition(0);
-   wait(10);
+   wait(1);
+   printf("DYNAMIXEL: Init 3 DONE");
 
-   printf("DYNAMIXEL: Init DONE\n\r");
+   printf("DYNAMIXEL: Init ALL DONE\n\r");
    
    button.rise(&button_int_handler);
   
    // CAN Initialization  
-   canrxa.start(canrx);
+   //canrxa.start(canrx);
 
-   printf("DONE: CAN Init\n\r");
-  
+   printf("CAN: Init DONE\n\r");
   
    printf("Running!\n\r");
   
    while(true)
    {
-      wait(1000);
+      canrx();
+      
+      if(desired_pose[0] != current_pose[0])
+      {
+        if(desired_pose[0] == 1)
+          motor_1.setSpeed(-SPEED);
+        else if(desired_pose[0] == 2)
+          motor_1.setSpeed(SPEED);
+        else
+          motor_1.setSpeed(0);
+          
+        current_pose[0] = desired_pose[0];
+      }
+      
+      if(desired_pose[1] != current_pose[1])
+      {
+        if(desired_pose[1] == 1)
+          motor_2.setSpeed(-SPEED);
+        else if(desired_pose[1] == 2)
+          motor_2.setSpeed(SPEED);
+        else
+          motor_2.setSpeed(0);
+          
+        current_pose[1] = desired_pose[1];
+      }
+      
+      if(desired_pose[2] != current_pose[2])
+      {
+        if(desired_pose[2] == 1)
+          motor_3.setSpeed(-SPEED);
+        else if(desired_pose[2] == 2)
+          motor_3.setSpeed(SPEED);
+        else
+          motor_3.setSpeed(0);
+          
+        current_pose[2] = desired_pose[2];
+      }
    }
 }