BASE
Dependencies: X-NUCLEO-IHM05A1
main.cpp
- Committer:
- gidiana
- Date:
- 2019-09-13
- Revision:
- 32:465e41868fe4
- Parent:
- 31:5d6a97adae07
- Child:
- 33:ba8992082097
File content as of revision 32:465e41868fe4:
#include "mbed.h" #include "L6208.h" #define VREFA_PWM_PIN D3 #define VREFB_PWM_PIN D9 #define SEND_FREQUENCY 10 //Hz l6208_init_t init = { 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes 80, //Acceleration current torque in % (from 0 to 100) 8000, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes 80, //Deceleration current torque in % (from 0 to 100) 8000, //Running speed in step/s or (1/16)th step/s for microstep modes 80, //Running current torque in % (from 0 to 100) 40, //Holding current torque in % (from 0 to 100) STEP_MODE_1_16, //Step mode via enum motorStepMode_t FAST_DECAY, //Decay mode via enum motorDecayMode_t 0, //Dwelling time in ms FALSE, //Automatic HIZ STOP 100000 //VREFA and VREFB PWM frequency (Hz) }; Thread cantxa(osPriorityNormal); Thread canrxa(osPriorityNormal); // Utility //InterruptIn button(USER_BUTTON); DigitalOut led(LED1); // Motor Control L6208 *motor; InterruptIn end1(USER_BUTTON, PullUp); DigitalIn end0(PA_5, PullUp); InterruptIn enc(PC_12, PullUp); 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; float pose, current_pose; float speed, current_speed; void zero() { printf("zero"); motor->run(StepperMotor::BWD); while(!end0){ } motor->hard_stop(); motor->set_home(); motor->go_to(0); printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); } uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id) { 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; } double to_rad(double angle) { return angle*0.0174533; } double angle_deparse (long int pose, float offset) { offset = offset * 0.00872664625; double angle = pose *0.000487012987; //do something 0,0004791666667 angle = (angle - offset); return angle; } void motor_error_handler(uint16_t error) { printf("ERROR: Motor Runtime\n\r"); } void end1_int_handler() { // motor->hard_stop(); motor->run(StepperMotor::FWD); printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); } // CAN CAN can1(PB_8, PB_9); // RX, TX CANMessage messageIn; CANMessage messageOut; void cantx () { while(1) { int _pose; messageOut.format = CANExtended; messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, BASE); pose=angle_deparse(motor->get_position(), 0); _pose=pose*100; messageOut.data[3]=_pose; messageOut.data[2]=_pose >>8; messageOut.data[1]=_pose >>16; messageOut.data[0]=_pose >>24; int status = can1.write(messageOut); led=!status; printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose); } } void cantx_ISR() { cantx(); osDelay(1/SEND_FREQUENCY*1000); } void canrx() { while(1) { // printf("receive\t\n"); if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE)) { speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24); printf("CAN: mess %d\n\r", speed); current_speed=speed; if (current_speed>0) { motor->set_max_speed(current_speed*80); motor->run(StepperMotor::FWD); } else if (current_speed<0) { motor->set_max_speed(current_speed*80); motor->run(StepperMotor::BWD); } else { motor->soft_stop(); current_pose= motor->get_position(); motor->go_to(current_pose); } } if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,BASE)) { if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1) { zero(); motor->wait_while_active(); } } } } void canrx_ISR() { canrx(); osDelay(10); } /* Main ----------------------------------------------------------------------*/ int main() { can1.frequency(125000); // Motor Initialization motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN); motor->set_step_mode(StepperMotor::STEP_MODE_1_16); if (motor->init(&init) != COMPONENT_OK) { printf("ERROR: vvMotor Init\n\r"); exit(EXIT_FAILURE); } motor->attach_error_handler(&motor_error_handler); end1.rise(&end1_int_handler); printf("DONE: Motor Init\n\r"); // CAN Initialization canrxa.start(canrx_ISR); //cantxa.start(cantx_ISR); printf("DONE: CAN Init\n\r"); printf("Running!\n\r"); //zero(); while(true) { wait(1000); } }