To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
Diff: main.cpp
- Revision:
- 19:7f8c174448d0
- Parent:
- 18:841ab8c1bdbd
- Child:
- 20:ff2dab77e3e9
--- a/main.cpp Sat Apr 06 17:00:28 2019 +0000 +++ b/main.cpp Tue Apr 16 08:59:06 2019 +0000 @@ -4,7 +4,7 @@ #include "AX12.h" #include "Servo.h" -#define SPEED 100 +#define SPEED 50 // Utility InterruptIn button(USER_BUTTON); @@ -36,225 +36,119 @@ int filter = can1.filter(0x000, 0x400, CANStandard); int pose; -int current_pose[6]; +int current_pose[] = {0, 0, 0, 0, 0, 0}; -void canrx() -{ - if(can1.read(messageIn, filter)) - { - 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) - { - //mutex.lock(); - current_pose[0]=pose; - //mutex.unlock(); - } - else if((messageIn.id & 0x0FF) == 0x50) - { - //mutex.lock(); - current_pose[1]=pose; - //mutex.unlock(); - } - - else if((messageIn.id & 0x0FF) == 0x60) - { - //mutex.lock(); - current_pose[2]=pose; - //mutex.unlock(); - } - else if((messageIn.id & 0x0FF) == 0x70) - { - //mutex.lock(); - current_pose[3]=pose; - //mutex.unlock(); - } - else if((messageIn.id & 0x0FF) == 0x80) - { - //mutex.lock(); - current_pose[4]=pose; - //mutex.unlock(); - } - else if((messageIn.id & 0x0FF) == 0x90) - { - //mutex.lock(); - current_pose[5]=pose; - //mutex.unlock(); - } - } - -} - int main() { -printf("DYNAMIXEL: Init \n\r"); -led=1; - wire.trigger(); - wire.trigger(); - wire.trigger(); - wire.trigger(); - wait(5); - // Setup Motor1 MultiTurn - motor_1.setMotorEnabled(1); - motor_1.setMode(0); - //motor_1.setSpeed(0); - //printf("Dynamixel 1 Position init: %f \n\r ", motor_1.getPosition()); - - wait(5); - printf("DYNAMIXEL: Init DONE 1\n\r"); - // Setup Motor2 MultiTurn - motor_2.setMotorEnabled(1); - motor_2.setMode(0); - //motor_2.setSpeed(0); - //printf("Dynamixel 2 Position init: %f \n\r ", motor_2.getPosition()); - - wait(5); - printf("DYNAMIXEL: Init DONE 2\n\r"); - // Setup Motor3 MultiTurn - motor_3.setMotorEnabled(1); - motor_3.setMode(0); - //motor_3.setSpeed(0); - //printf("Dynamixel 3 Position init: %f \n\r ", motor_3.getPosition()); - wait(5); - printf("DYNAMIXEL: Init DONE 3\n\r"); - //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(5); + can1.frequency(125000); + printf("CAN: Init DONE\n\r"); + + wire.trigger(); + wire.trigger(); + wire.trigger(); + wire.trigger(); + wait(1); - printf("DYNAMIXEL: Init DONE\n\r"); - - button.rise(&button_int_handler); - cam1=0,5; - cam2=0,5; - // CAN Initialization - //canrxa.start(canrx); + // Setup Motor1 MultiTurn + motor_1.setMotorEnabled(1); + motor_1.setMode(0); + wait(3); + printf("DYNAMIXEL: Init DONE 1\n\r"); + + // Setup Motor2 MultiTurn + motor_2.setMotorEnabled(1); + motor_2.setMode(0); + wait(3); + printf("DYNAMIXEL: Init DONE 2\n\r"); - printf("DONE: CAN Init\n\r"); - - - printf("Running!\n\r"); - - while(true) - { - canrx(); - if (current_pose[0]==1) + // Setup Motor3 MultiTurn + motor_3.setMotorEnabled(1); + motor_3.setMode(0); + wait(3); + printf("DYNAMIXEL: Init DONE 3\n\r"); + + // Setup Motor4 MultiTurn + motor_4.setMotorEnabled(1); + motor_4.setMode(0); + wait(3); + printf("DYNAMIXEL: Init DONE 4\n\r"); + + printf("Running!\n\r"); + + while(true) + { + if(can1.read(messageIn, filter)) + { + pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); + + if(messageIn.id == 0x40 && pose != current_pose[0]) { - motor_1.setSpeed(-SPEED); - printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); - } - - else if (current_pose[0]==0) - { - motor_1.setSpeed(0); - printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); - } - else if (current_pose[0]==2) - { - - motor_1.setSpeed(SPEED); - printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); + if(pose == 1) + { + motor_1.setSpeed(-SPEED); + } + else if(pose == 2) + { + motor_1.setSpeed(SPEED); + } + else + { + motor_1.setSpeed(0); + } - } - - if (current_pose[1]==1) - { - 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 (current_pose[1]==0) - { - motor_2.setSpeed(0); - printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); + current_pose[0] = pose; } - else if (current_pose[1]==2) + else if(messageIn.id == 0x50 && pose != current_pose[1]) { - - motor_2.setSpeed(SPEED); - printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); - if (motor_2.getPosition()>90) motor_2.setSpeed(0); - } - - if (current_pose[2]==1) - { - motor_3.setSpeed(-SPEED); - printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); - } - - else if (current_pose[2]==0) - { - motor_3.setSpeed(0); - printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); + if(pose == 1) + { + motor_2.setSpeed(-SPEED); + } + else if(pose == 2) + { + motor_2.setSpeed(SPEED); + } + else + { + motor_2.setSpeed(0); + } + + current_pose[1] = pose; } - else if (current_pose[2]==2) - { - - motor_3.setSpeed(SPEED); - printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); - - } - - if (current_pose[3]==1) - { - motor_4.setSpeed(-SPEED); - printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); - } - - else if (current_pose[3]==0) - { - motor_4.setSpeed(0); - printf("Dynamixel 4 Position : %f \n\r ", motor_1.getPosition()); - } - else if (current_pose[3]==2) + else if(messageIn.id == 0x60 && pose != current_pose[2]) { - - motor_4.setSpeed(SPEED); - printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); - } - - if (current_pose[4]==1) - { - if (cam1==0) - cam1=cam1; - else cam1 = cam1-0.2; - - wait(0.2); - - } - else if (current_pose[4]==2) - { - - if (cam1==1) - cam1=cam1; - else cam1 = cam1+0.2; + if(pose == 1) + { + motor_3.setSpeed(-SPEED); + } + else if(pose == 2) + { + motor_3.setSpeed(SPEED); + } + else + { + motor_3.setSpeed(0); + } - wait(0.2); - } - - if (current_pose[5]==1) - { - if (cam2==0) - cam1=cam1; - else cam2 = cam2-0.2; - - wait(0.2); - + current_pose[2] = pose; } - else if (current_pose[5]==2) + else if(messageIn.id == 0x70 && pose != current_pose[3]) { - - if (cam2==1) - cam2=cam2; - else cam2 = cam2+0.2; + if(pose == 1) + { + motor_4.setSpeed(-SPEED); + } + else if(pose == 2) + { + motor_4.setSpeed(SPEED); + } + else + { + motor_4.setSpeed(0); + } - wait(0.2); - } - } -} + current_pose[3] = pose; + } + } + } +} \ No newline at end of file