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