To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
main.cpp
- Committer:
- stebonicelli
- Date:
- 2019-02-08
- Revision:
- 13:698bd4df9702
- Parent:
- 11:19e8022f60ea
- Child:
- 14:c51c4e0f3bc9
File content as of revision 13:698bd4df9702:
#include "mbed.h" #include "communication_1.h" #include "MX106.h" // Utility InterruptIn button(USER_BUTTON); DigitalOut led(LED1); // Motor Control Serial pc(USBTX, USBRX); communication_1 wire(PA_9, PA_10, 200000); MX106 motor_1(wire, 1, 1); MX106 motor_2(wire, 2, 1); MX106 motor_3(wire, 3, 1); 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); void canrx() { while(1) { if(can1.read(messageIn, filter)) { printf("CAN: mess %d\n\r", (messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))); printf("CANaacc: id %x \n\r ",messageIn.id); if((messageIn.id & 0x0FF) == 0x40) { } else if((messageIn.id & 0x0FF) == 0x50) { } else if((messageIn.id & 0x0FF) == 0x60) { } else if((messageIn.id & 0x0FF) == 0x70) { } } } } int main() { wire.trigger(); wire.trigger(); wire.trigger(); wire.trigger(); // Setup Motor1 MultiTurn motor_1.setMotorEnabled(1); motor_1.setMode(2); motor_1.setSpeed(90); //motor_1.setGoalPosition(0); wait(10); // Setup Motor2 MultiTurn motor_2.setMotorEnabled(1); motor_2.setMode(2); motor_2.setSpeed(90); //motor_2.setGoalPosition(0); wait(10); // Setup Motor3 MultiTurn motor_3.setMotorEnabled(1); motor_3.setMode(2); motor_3.setSpeed(90); //motor_3.setGoalPosition(0); wait(10); printf("DYNAMIXEL: Init DONE\n\r"); button.rise(&button_int_handler); // CAN Initialization canrxa.start(canrx); printf("DONE: CAN Init\n\r"); printf("Running!\n\r"); while(true) { wait(1000); } }