FOrk
Dependencies: Servo AX12_final MX106_not_working comunication_1
Diff: main.cpp
- Revision:
- 18:841ab8c1bdbd
- Parent:
- 17:2ab8feddb7c8
- Child:
- 19:7f8c174448d0
diff -r 2ab8feddb7c8 -r 841ab8c1bdbd main.cpp --- a/main.cpp Sat Feb 09 16:53:04 2019 +0000 +++ b/main.cpp Sat Apr 06 17:00:28 2019 +0000 @@ -3,20 +3,24 @@ #include "MX106.h" #include "AX12.h" #include "Servo.h" + #define SPEED 100 + // Utility InterruptIn button(USER_BUTTON); DigitalOut led(LED1); // Motor Control -//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); + +// Camera PanTilt Control Servo cam1 (D9); Servo cam2 (D10); + void button_int_handler() { @@ -24,15 +28,16 @@ // CAN //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=1; + +int pose; int current_pose[6]; + void canrx() { if(can1.read(messageIn, filter)) @@ -137,20 +142,19 @@ while(true) { - canrx(); - if (current_pose[0]==0) + if (current_pose[0]==1) { motor_1.setSpeed(-SPEED); printf("Dynamixel 1 Position : %f \n\r ", motor_1.getPosition()); } - else if (current_pose[0]==50) + 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]==100) + else if (current_pose[0]==2) { motor_1.setSpeed(SPEED); @@ -158,19 +162,19 @@ } - else if (current_pose[1]==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]==50) + else if (current_pose[1]==0) { motor_2.setSpeed(0); printf("Dynamixel 2 Position : %f \n\r ", motor_2.getPosition()); } - else if (current_pose[1]==100) + else if (current_pose[1]==2) { motor_2.setSpeed(SPEED); @@ -178,18 +182,18 @@ if (motor_2.getPosition()>90) motor_2.setSpeed(0); } - else if (current_pose[2]==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]==50) + else if (current_pose[2]==0) { motor_3.setSpeed(0); printf("Dynamixel 3 Position : %f \n\r ", motor_3.getPosition()); } - else if (current_pose[2]==100) + else if (current_pose[2]==2) { motor_3.setSpeed(SPEED); @@ -197,25 +201,25 @@ } - else if (current_pose[3]==0) + if (current_pose[3]==1) { motor_4.setSpeed(-SPEED); printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); } - else if (current_pose[3]==50) + 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]==100) + else if (current_pose[3]==2) { motor_4.setSpeed(SPEED); printf("Dynamixel 4 Position : %f \n\r ", motor_4.getPosition()); } - else if (current_pose[4]==0) + if (current_pose[4]==1) { if (cam1==0) cam1=cam1; @@ -224,7 +228,7 @@ wait(0.2); } - else if (current_pose[4]==100) + else if (current_pose[4]==2) { if (cam1==1) @@ -233,7 +237,8 @@ wait(0.2); } - else if (current_pose[5]==0) + + if (current_pose[5]==1) { if (cam2==0) cam1=cam1; @@ -242,7 +247,7 @@ wait(0.2); } - else if (current_pose[5]==100) + else if (current_pose[5]==2) { if (cam2==1) @@ -250,8 +255,6 @@ else cam2 = cam2+0.2; wait(0.2); - } - - + } } }