![](/media/cache/group/Diana_su_nero.jpg.50x50_q85.jpg)
Basic code for linear actuator and end effector motor
Dependencies: QEI X_NUCLEO_IHM04A1 arm_linear_can_2
Diff: main.cpp
- Revision:
- 29:93a31c16467b
- Parent:
- 28:637b6f726971
- Child:
- 30:62e21e5b4445
diff -r 637b6f726971 -r 93a31c16467b main.cpp --- a/main.cpp Fri Jun 21 07:56:33 2019 +0000 +++ b/main.cpp Sat Sep 14 20:33:53 2019 +0000 @@ -1,20 +1,58 @@ #include "mbed.h" #include "L6206.h" +#include "BDCMotor.h" +#include <math.h> -#define JOINT_SET_SPEED 20 +/****************************/ +/* PIN DEFINITION */ +/****************************/ + +// CAN +#define CAN_RX PB_8 +#define CAN_TX PB_9 + +// ENCODER +#define CH_A PA_8 +#define CH_B PA_9 + +/********************************/ +/* CAN */ +/********************************/ -#define JOINT_ID 3 +typedef enum +{ + JOINT_SET_SPEED = 20, + JOINT_SET_POSITION, + JOINT_CURRENT_POSITION, + JOINT_CURRENT_SPEED, + JOINT_STATUS, + JOINT_ERROR, + JOINT_TORQUE, + JOINT_MAXTORQUE, + JOINT_ZERO, +} CAN_COMMANDS; +typedef enum +{ + BASE = 1, + SHOULDER, + ELBOW, + WRIST1, + WRIST2, + WRIST3, + END_EFFECTOR, + CAMERA1, + CAMERA2, +} JOINT; + +// static volatile uint16_t gLastError; static volatile uint8_t gStep = 0; -int current_pose = 0; -int speed = 0; - L6206_init_t init = { - PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B, + L6206_CONF_PARAM_PARALLE_BRIDGES, {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B}, {100,100,100,100}, {FORWARD,BACKWARD,FORWARD,BACKWARD}, @@ -22,19 +60,18 @@ {FALSE,FALSE} }; -L6206 *motor; - -Thread canrxa; - -//Utility -DigitalOut led(LED1); //Change? +// Motor definition +L6206 *LinAct; +L6206 *EndEff; -void motor_zero() -{ - motor->run(0, BDCMotor::FWD); - motor->run(1, BDCMotor::FWD); -} +int speed_elbow = 0; +int speed_ee = 0; +/*********************************/ +/* Interrupt Handlers */ +/*********************************/ + +// Error Handler (called by the library when it reports an error) void my_error_handler(uint16_t error) { /* Backup error number */ @@ -43,118 +80,151 @@ /* Enter your own code here */ } +// Flag Handler (overcurrent and thermal alarms reporting) void my_flag_irq_handler(void) { - /* Code to be customised */ - /************************/ - /* Get the state of bridge A */ - uint16_t bridgeState = motor->get_bridge_status(0); - - if (bridgeState == 0) { - if ((motor->get_device_state(0) != INACTIVE)|| - (motor->get_device_state(1) != INACTIVE)) { - /* Bridge A was disabling due to overcurrent or over temperature */ - /* When at least on of its motor was running */ - my_error_handler(0XBAD0); + /* Get the state of bridge A */ + uint16_t bridgeState = EndEff->get_bridge_status(0); + + if (bridgeState == 0) + { + if ((EndEff->get_device_state(0) != INACTIVE)|| + (EndEff->get_device_state(1) != INACTIVE)) + { + /* Bridge A was disabling due to overcurrent or over temperature */ + /* When at least on of its motor was running */ + my_error_handler(0XBAD0); + } } - } - /* Get the state of bridge B */ - bridgeState = motor->get_bridge_status(1); + /* Get the state of bridge B */ + bridgeState = LinAct->get_bridge_status(1); - if (bridgeState == 0) { - if ((motor->get_device_state(2) != INACTIVE)|| - (motor->get_device_state(3) != INACTIVE)) { - /* Bridge A was disabling due to overcurrent or over temperature */ - /* When at least on of its motor was running */ - my_error_handler(0XBAD1); - } - } + if (bridgeState == 0) + { + if ((LinAct->get_device_state(2) != INACTIVE)|| + (LinAct->get_device_state(3) != INACTIVE)) + { + /* Bridge A was disabling due to overcurrent or over temperature */ + /* When at least on of its motor was running */ + my_error_handler(0XBAD1); + } + } } -// CAN, to revise +/****************************/ +/* CAN Interface */ +/****************************/ + CAN can1(PB_8, PB_9); // RX, TX -CANMessage messageIn; -CANMessage messageOut; +Thread t_canrx, t_cantx; -void canrx() +uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id) { - while(1) - { - if(can1.read(messageIn)) + uint32_t id = (uint32_t)can_id; // LSB byte is the controller id. + id |= (uint32_t)message_id << 8; // Next lowest byte is the packet id. + id |= 0x80000000; // Send in Extended Frame Format. + return id; +} + +bool can_rx() +{ + CANMessage messageIn; + messageIn.format = CANExtended; + bool status = can1.read(messageIn); + printf ("CAN ID : %d Message received : %d\n\r", messageIn.id, status); + + if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_SET_SPEED, ELBOW)) { - printf("READ!\n\r"); - - if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID)) - { - speed = 0; - speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); - - motor->set_speed(0, speed); - (speed > 0) ? motor->run(0, BDCMotor::BWD) : motor->run(0, BDCMotor::FWD); - - printf("CAN: mess %d\n\r", speed); - } + speed_elbow = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); + } + + if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_SET_SPEED, END_EFFECTOR)) + { + speed_ee = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); + } + + if(can1.read(messageIn) && messageIn.id == gen_can_id(JOINT_ZERO,ELBOW)) + { + if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)) == 1) + { + LinAct->run(1, BDCMotor::BWD); + } } - int speed = 5000; - - messageOut.data[0] = speed >> 24; - messageOut.data[1] = speed >> 16; - messageOut.data[2] = speed >> 8; - messageOut.data[3] = speed; - - messageOut.id = (3 << 8) + 2; - int status = can1.write(messageOut); - - printf("STATUS: %d\n\r", status); - - wait(0.01); - } + return status; } +void can_rx_isr() +{ + while(1) + { + can_rx(); + osDelay(10); + } +} -/* Main ----------------------------------------------------------------------*/ +/*****************************/ +/* MAIN */ +/*****************************/ + int main() { - can1.frequency(125000); - messageIn.format=CANExtended; - messageOut.format=CANExtended; - - - // Motor Initialization - + can1.frequency(125000); + +// Motor Initialization #ifdef TARGET_STM32F429 - motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3); + LinAct = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7); // EN_A, EN_B, IN1_A, IN2_A, IN1_B, IN2_B + EndEff = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7); // EN_A, EN_B, IN1_A, IN2_A, IN1_B, IN2_B #else - motor = new L6206(D2, A4, D5, D4, A0, A1); + //LinAct = new L6206(PB_14, PB_15, PA_8, PA_9, PC_6, PC_7); + LinAct = new L6206(D2, A4, D5, D4, A0, A1); + EndEff = new L6206(D2, A4, D5, D4, A0, A1); #endif - - if (motor->init(&init) != COMPONENT_OK) - { - printf("ERROR: vvMotor Init\n\r"); - exit(EXIT_FAILURE); - } - motor->attach_flag_interrupt(my_flag_irq_handler); - motor->attach_error_handler(my_error_handler); + if (LinAct->init(&init) != COMPONENT_OK) + { + printf("ERROR: vvMotor Init\n\r"); + exit(EXIT_FAILURE); + } + if (EndEff->init(&init) != COMPONENT_OK) + { + printf("ERROR: vvMotor Init\n\r"); + exit(EXIT_FAILURE); + } + + LinAct->attach_flag_interrupt(my_flag_irq_handler); + LinAct->attach_error_handler(my_error_handler); + EndEff->attach_flag_interrupt(my_flag_irq_handler); + EndEff->attach_error_handler(my_error_handler); + printf("DONE: Motor Init\n\r"); + + LinAct->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B); + EndEff->set_dual_full_bridge_config(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B); - printf("DONE: Motor Init\n\r"); - - // CAN Initialization - - canrxa.start(canrx); - - printf("DONE: CAN Init\n\r"); - - //motor->set_speed(1, 50); - //motor->run(1, BDCMotor::FWD); - - printf("Running!\n\r"); - - while(true) - { - wait(1); - } -} + // CAN Initialization + t_canrx.start(can_rx_isr); + printf("DONE: CAN Init\n\r"); + + while(true) + { + EndEff->set_speed(0, speed_ee); + if(speed_ee < 0) + EndEff->run(0, BDCMotor::BWD); + else if(speed_ee > 0) + EndEff->run(0, BDCMotor::FWD); + else if(speed_ee == 0) + EndEff->hard_stop(0); + + LinAct->set_speed(1, speed_elbow); + if(speed_elbow < 0) + LinAct->run(1, BDCMotor::BWD); + else if(speed_elbow > 0) + LinAct->run(1, BDCMotor::FWD); + else if(speed_elbow == 0) + LinAct->hard_stop(1); + + osDelay(100); + } +} \ No newline at end of file