To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
main.cpp
- Committer:
- gidiana
- Date:
- 2019-06-21
- Revision:
- 21:43740448011a
- Parent:
- 20:ff2dab77e3e9
- Child:
- 22:ffb26af4d5d8
File content as of revision 21:43740448011a:
#include "mbed.h" #include "communication_1.h" #include "MX106.h" #include "AX12.h" #include "Servo.h" #define SPEED 50 #define JOINT_SET_SPEED 20 #define JOINT1_ID 4 #define JOINT2_ID 5 #define JOINT3_ID 6 #define JOINT4_ID 7 // 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 (PC_0); //Servo cam2 (PC_1); void button_int_handler() { } // CAN //Thread canrxa; CAN can1(PB_5, PB_6); // RX, TX CANMessage messageIn; CANMessage messageOut; int pose; int current_pose[] = {0, 0, 0, 0, 0, 0}; int main() { can1.frequency(125000); messageIn.format=CANExtended; 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"); int32_t speed = 0; while(true) { if(can1.read(messageIn)) { if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT1_ID)) { speed = 0; speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); motor_1.setSpeed(speed); printf("CAN: mess %d\n\r", speed); } if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT2_ID)) { speed = 0; speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); motor_2.setSpeed(speed); printf("CAN: mess %d\n\r", speed); } if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT3_ID)) { speed = 0; speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); motor_3.setSpeed(speed); printf("CAN: mess %d\n\r", speed); } if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT4_ID)) { speed = 0; speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); motor_4.setSpeed(speed); printf("CAN: mess %d\n\r", speed); } } wait(0.1); } }