Diff: main.cpp

Revision:

16:5454456b36f7

Parent:

15:d058877eb501

Child:

17:2ab8feddb7c8

--- a/main.cpp	Fri Feb 08 19:31:04 2019 +0000
+++ b/main.cpp	Fri Feb 08 20:36:19 2019 +0000
@@ -2,6 +2,7 @@
 #include "communication_1.h"
 #include "MX106.h"
 #include "AX12.h"
+#include "Servo.h"
 #define SPEED 100
 // Utility
 InterruptIn button(USER_BUTTON);
@@ -14,7 +15,8 @@
 MX106 motor_2(wire, 2, 1);
 MX106 motor_3(wire, 3, 1);
 AX12  motor_4(wire, 4, 1);
-
+Servo cam1 (PB_12);
+Servo cam2 (PB_13);
 void button_int_handler()
 {
 
@@ -129,11 +131,49 @@
             {
                
                 motor_1.setSpeed(SPEED);
-                printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());
-                
+                printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition());    
             }
             else 
                 motor_4.setSpeed(0); 
+    }
+        else if((messageIn.id & 0x0FF) == 0x80)
+        {
+            if (pose==0)
+            {
+                for(float p=0; p<1.0; p -= 0.1) 
+                {
+                cam1 = p;
+                wait(0.2);
+                }
+            }
+            else if (pose==100)
+            {
+                for(float p=0; p<1.0; p += 0.1) 
+                {
+                cam1 = p;
+                wait(0.2);
+                }
+            } 
+        }
+         else if((messageIn.id & 0x0FF) == 0x90)
+        {
+            if (pose==0)
+            {
+                for(float p=0; p<1.0; p -= 0.1) 
+                {
+                cam2 = p;
+                wait(0.2);
+                }
+          }
+            else if (pose==100)
+            {
+                for(float p=0; p<1.0; p += 0.1) 
+                {
+                cam2 = p;
+                wait(0.2);
+                }
+            }
+                     
       }
     }
   }
@@ -169,7 +209,7 @@
    printf("Dynamixel 3 Position init: %f \n\r ", motor_3.getPosition());
    wait(10);
    //motor_3.setGoalPosition(0);
-     // Setup Motor4 MultiTurn
+   // Setup Motor4 MultiTurn
    motor_4.setMotorEnabled(1);
    motor_4.setMode(0);
    motor_4.setSpeed(0);