To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
Diff: main.cpp
- Revision:
- 13:698bd4df9702
- Parent:
- 11:19e8022f60ea
- Child:
- 14:c51c4e0f3bc9
--- a/main.cpp Thu Nov 24 15:29:50 2016 +0000 +++ b/main.cpp Fri Feb 08 15:23:40 2019 +0000 @@ -1,154 +1,104 @@ #include "mbed.h" #include "communication_1.h" #include "MX106.h" -#include "MX64.h" -#include <stdio.h> -#include <string.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); -#define MAX_JOINT 3 +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); -/*Connessioni jupers millefori/nucleo: - - rosso 5V - - nero GND - - marrone D2 - - verde D8 - Il connettore va stacato dal dynamixel e non dalla millefori per non scambiare il verso!*/ - -Serial pc(USBTX, USBRX); -communication_1 wire(PA_9, PA_10, 9600); -MX106 motor_21(wire, 21, 1); -MX106 motor_22(wire, 22, 1); -MX106 motor_14(wire, 14, 1); +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() { - //pc.printf("Start run! \n"); +int main() +{ wire.trigger(); wire.trigger(); wire.trigger(); wire.trigger(); - //printf("asking volts\n"); - //float volts = motor_21.getVolts(); - //printf("volts: %f\n", volts); + // Setup Motor1 MultiTurn + motor_1.setMotorEnabled(1); + motor_1.setMode(2); + motor_1.setSpeed(90); + //motor_1.setGoalPosition(0); + wait(10); - motor_21.setMotorEnabled(1); - - //printf("set continuous rotation mode\n"); - //motor_21.setMode(0); - //printf("set speed\n"); - //motor_21.setSpeed(180); - //printf("speed set\n"); - - - //printf("set multiturn mode\n"); - //motor_21.setMode(2); - //printf("asking pos\n"); - //float pos = motor_21.getPosition(); - //printf(" pos : %f \n", pos); - //motor_21.setSpeed(90); - //float wait_time = 5; - //motor_21.setGoalPosition(0); - //wait(10); - //float targets[7] = {0, 90, 180, 270, 180, 360, 720}; - //for (int i = 0; i < 7; i++) { - //motor_21.setGoalPosition(targets[i]); - //wait(wait_time); - //} - //printf("asking pos after move\n"); - //pos = motor_21.getPosition(); - //printf(" pos : %f \n", pos); - - Joint* joint_21 = &motor_21; - - float pos; - //joint_21->setMaxSpeed(360*3/2); - //joint_21->setSpeed(90); - //wait(3); - //joint_21->setSpeed(0); - //wait(1); - //pos = motor_21.getPosition(); - //printf(" pos : %f \n", pos); + // Setup Motor2 MultiTurn + motor_2.setMotorEnabled(1); + motor_2.setMode(2); + motor_2.setSpeed(90); + //motor_2.setGoalPosition(0); + wait(10); - - //joint_21->setCWLimit(90); - //joint_21->setCCWLimit(-90); - //joint_21->setGoalPosition(0); - //wait(5); - //pos = motor_21.getPosition(); - //printf(" pos : %f \n", pos); - - //joint_21->setGoalPosition(80); - //wait(5); - //pos = motor_21.getPosition(); - //printf(" pos : %f \n", pos); - - //joint_21->setGoalPosition(130); - //wait(5); - //pos = motor_21.getPosition(); - //printf(" pos : %f \n", pos); + // Setup Motor3 MultiTurn + motor_3.setMotorEnabled(1); + motor_3.setMode(2); + motor_3.setSpeed(90); + //motor_3.setGoalPosition(0); + wait(10); - //joint_21->setGoalPosition(-130); - //wait(5); - //pos = motor_21.getPosition(); - //printf(" pos : %f \n", pos); - // - // + printf("DYNAMIXEL: Init DONE\n\r"); - Joint* joints[MAX_JOINT]; - joints[0] = &motor_21; - joints[1] = &motor_22; - joints[2] = &motor_14; - - // comandi - char buf[200]; - uint8_t current_motor = 0; - - while(1) { - printf("\n insert command: \n"); - if(pc.gets(buf, 200)) { - char command[3]; - strncpy(command, buf, 2); - command[2]='\0'; + button.rise(&button_int_handler); + + // CAN Initialization + canrxa.start(canrx); - if(strcmp(command, "ID") == 0) { - int id; - if(sscanf(buf, "ID %d", &id) != 1) { - printf("\n bad ID command \n"); - }; - bool found = false; - for(int i =0; i< MAX_JOINT && !found; i++) { - int nid = joints[i]->getID(); - printf("found motor with id %d\n", nid); - if(id == nid) { - printf("\nusing motor %d - ID: %d", i, id); - current_motor = i; - found = true; - break; - } - } - if(!found) { - printf("\nmotor with ID: %d not found", id); - } - } else if(strcmp(command, "GO") == 0) { - float posDegrees; - if(sscanf(buf, "GO %f", &posDegrees) != 1) { - printf("\n bad GO command \n"); - break; - } - joints[current_motor]->setGoalPosition(posDegrees); - } else if(strcmp(command, "SP") == 0) { - float speed; - if(sscanf(buf, "SP %f", &speed) != 1) { - printf("\n bad SP command \n"); - break; - } - joints[current_motor]->setSpeed(speed); - } else { - printf("\n unknown command: \"\s\" \n", buf); - } - - - }; + printf("DONE: CAN Init\n\r"); + + + printf("Running!\n\r"); + + while(true) + { + wait(1000); } - }