To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
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);