To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
main.cpp
- Committer:
- stebonicelli
- Date:
- 2019-04-16
- Revision:
- 20:ff2dab77e3e9
- Parent:
- 19:7f8c174448d0
- Child:
- 21:43740448011a
File content as of revision 20:ff2dab77e3e9:
#include "mbed.h" #include "communication_1.h" #include "MX106.h" #include "AX12.h" #include "Servo.h" #define SPEED 50 // Utility InterruptIn button(USER_BUTTON); DigitalOut led(LED1); // Motor Control 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() { } // CAN //Thread canrxa; CAN can1(PA_11, PA_12); // RX, TX CANMessage messageIn; CANMessage messageOut; int filter = can1.filter(0x000, 0x400, CANStandard); int pose; int current_pose[] = {0, 0, 0, 0, 0, 0}; int main() { can1.frequency(125000); printf("CAN: Init DONE\n\r"); wire.trigger(); wire.trigger(); wire.trigger(); wire.trigger(); wait(1); // 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"); // 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]) { if(pose == 1) { motor_1.setSpeed(-SPEED); } else if(pose == 2) { motor_1.setSpeed(SPEED); } else { motor_1.setSpeed(0); } current_pose[0] = pose; } else if(messageIn.id == 0x50 && pose != current_pose[1]) { 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(messageIn.id == 0x60 && pose != current_pose[2]) { if(pose == 1) { motor_3.setSpeed(-SPEED); } else if(pose == 2) { motor_3.setSpeed(SPEED); } else { motor_3.setSpeed(0); } current_pose[2] = pose; } else if(messageIn.id == 0x70 && pose != current_pose[3]) { if(pose == 1) { motor_4.setSpeed(-SPEED); } else if(pose == 2) { motor_4.setSpeed(SPEED); } else { motor_4.setSpeed(0); } current_pose[3] = pose; } } } }