FOrk
Dependencies: Servo AX12_final MX106_not_working comunication_1
Diff: main.cpp
- Revision:
- 17:2ab8feddb7c8
- Parent:
- 16:5454456b36f7
- Child:
- 18:841ab8c1bdbd
diff -r 5454456b36f7 -r 2ab8feddb7c8 main.cpp --- a/main.cpp Fri Feb 08 20:36:19 2019 +0000 +++ b/main.cpp Sat Feb 09 16:53:04 2019 +0000 @@ -9,33 +9,32 @@ DigitalOut led(LED1); // Motor Control -Serial pc(USBTX, USBRX); -communication_1 wire(PA_9, PA_10, 200000); +//Serial pc(USBTX, USBRX); +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); AX12 motor_4(wire, 4, 1); -Servo cam1 (PB_12); -Servo cam2 (PB_13); +Servo cam1 (D9); +Servo cam2 (D10); void button_int_handler() { } // CAN -Thread canrxa; - +//Thread canrxa; +//Mutex mutex; CAN can1(PA_11, PA_12); // RX, TX CANMessage messageIn; CANMessage messageOut; int filter = can1.filter(0x000, 0x400, CANStandard); -int pose; +int pose=1; +int current_pose[6]; void canrx() { - while(1) - { if(can1.read(messageIn, filter)) { pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); @@ -44,184 +43,92 @@ 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(SPEED); - printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); - - } - else - motor_1.setSpeed(0); + //mutex.lock(); + current_pose[0]=pose; + //mutex.unlock(); } 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(SPEED); - printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); - if (motor_2.getPosition()>90) motor_2.setSpeed(0); - } - else - motor_1.setSpeed(0); + //mutex.lock(); + current_pose[1]=pose; + //mutex.unlock(); } + 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(SPEED); - printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); - - } - else - motor_3.setSpeed(0); + //mutex.lock(); + current_pose[2]=pose; + //mutex.unlock(); } 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(SPEED); - printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); - } - else - motor_4.setSpeed(0); - } + //mutex.lock(); + current_pose[3]=pose; + //mutex.unlock(); + } 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); - } - } + //mutex.lock(); + current_pose[4]=pose; + //mutex.unlock(); } 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); - } - } - - } + //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()); - //motor_1.setGoalPosition(0); - wait(10); - + //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()); - //motor_2.setGoalPosition(0); - wait(10); + //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(10); + //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(10); - + //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); + printf("DYNAMIXEL: Init DONE\n\r"); button.rise(&button_int_handler); - + cam1=0,5; + cam2=0,5; // CAN Initialization - canrxa.start(canrx); + //canrxa.start(canrx); printf("DONE: CAN Init\n\r"); @@ -230,6 +137,121 @@ while(true) { - wait(1000); + + canrx(); + if (current_pose[0]==0) + { + motor_1.setSpeed(-SPEED); + printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); + } + + else if (current_pose[0]==50) + { + motor_1.setSpeed(0); + printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); + } + else if (current_pose[0]==100) + { + + motor_1.setSpeed(SPEED); + printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); + + } + + else if (current_pose[1]==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 (current_pose[1]==50) + { + motor_2.setSpeed(0); + printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); + } + else if (current_pose[1]==100) + { + + 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[2]==0) + { + motor_3.setSpeed(-SPEED); + printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); + } + + else if (current_pose[2]==50) + { + motor_3.setSpeed(0); + printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); + } + else if (current_pose[2]==100) + { + + motor_3.setSpeed(SPEED); + printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); + + } + + else if (current_pose[3]==0) + { + motor_4.setSpeed(-SPEED); + printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); + } + + else if (current_pose[3]==50) + { + motor_4.setSpeed(0); + printf("Dynamixel 4 Position : %f \n\r ", motor_1.getPosition()); + } + else if (current_pose[3]==100) + { + + motor_4.setSpeed(SPEED); + printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); + } + + else if (current_pose[4]==0) + { + if (cam1==0) + cam1=cam1; + else cam1 = cam1-0.2; + + wait(0.2); + + } + else if (current_pose[4]==100) + { + + if (cam1==1) + cam1=cam1; + else cam1 = cam1+0.2; + + wait(0.2); + } + else if (current_pose[5]==0) + { + if (cam2==0) + cam1=cam1; + else cam2 = cam2-0.2; + + wait(0.2); + + } + else if (current_pose[5]==100) + { + + if (cam2==1) + cam2=cam2; + else cam2 = cam2+0.2; + + wait(0.2); + } + + } }