BASE
Dependencies: X-NUCLEO-IHM05A1
Diff: main.cpp
- Revision:
- 29:f888a2394027
- Parent:
- 28:8878dd50b7e1
- Child:
- 30:c40b060795a2
diff -r 8878dd50b7e1 -r f888a2394027 main.cpp --- a/main.cpp Fri Jun 21 07:58:24 2019 +0000 +++ b/main.cpp Wed Aug 07 12:30:41 2019 +0000 @@ -4,10 +4,6 @@ #define VREFA_PWM_PIN D3 #define VREFB_PWM_PIN D9 -#define JOINT_SET_SPEED 20 - -#define JOINT_ID 2 - l6208_init_t init = { 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes @@ -25,6 +21,7 @@ }; Thread canrxa; +Thread cantxa; // Utility InterruptIn button(USER_BUTTON); @@ -33,46 +30,78 @@ // Motor Control L6208 *motor; -InterruptIn end0(PC_4, PullUp); -InterruptIn end1(PC_8, PullUp); +InterruptIn end0(PA_5, PullUp); +InterruptIn end1(PA_6, PullUp); InterruptIn enc(PC_12, PullUp); -int32_t speed = 0; +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; -void motor_error_handler(uint16_t error) +typedef enum { - printf("ERROR: Motor Runtime\n\r"); - while(1){} -} + BASE=1, + SHOULDER, + ELBOW, + WRIST1, + WRIST2, + WRIST3, + END_EFFECTOR, + CAMERA1, + CAMERA2, +}JOINT; -void motor_zero() +long int pose, current_pose; +int speed, current_speed; + +uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id) { - motor->run(StepperMotor::BWD); + 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; } -void button_int_handler() +int angle_deparse (long int pose, float offset) { - printf("POSITION: %d\n\r", motor->get_position()); - motor_zero(); + offset = offset * 0.00872664625; + float angle = pose * 0.00000425 ; //do something + angle = (angle - offset) * 100; + return (int)angle; } - -void end0_pressed() +void motor_error_handler(uint16_t error) { - motor->run(StepperMotor::BWD); - printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); + printf("ERROR: Motor Runtime\n\r"); + } -void end0_released() +void end0_int_handler() { - motor->go_to(motor->get_position()); + motor->hard_stop(); + + motor->run(StepperMotor::FWD); + + printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); + + } -void end1_pressed() +void end1_int_handler() { - motor->hard_stop(); - motor->run(StepperMotor::FWD); + motor->hard_stop(); - printf("END1: Pressed\n\r"); + motor->run(StepperMotor::BWD); + + printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); } void motor_set_home() @@ -80,6 +109,7 @@ motor->hard_stop(); motor->set_home(); motor->go_to(0); + } // CAN @@ -88,26 +118,68 @@ CANMessage messageIn; CANMessage messageOut; +void cantx () +{ + messageOut.format = CANExtended; + messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, BASE); + pose=angle_deparse(motor->get_position(), 0); + + messageOut.data[3]=pose; + messageOut.data[2]=pose >>8; + messageOut.data[1]=pose >>16; + messageOut.data[0]=pose >>24; + + int status = can1.write(messageOut); + printf("CAN send CURRENT POSITION Joint status %d : pose %d",status, pose); + + wait(1); + + + +} + + + void canrx() { while(1) - { - if(can1.read(messageIn)) + { + if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE)) { - printf("CAN: message received\r\n"); - if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID)) + speed=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); + printf("CAN: mess %d\n\r", speed); + current_speed=speed-100; + + + if (current_speed>0) { - speed = 0; - speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); - - motor->set_max_speed(speed); - (speed > 0) ? motor->run(StepperMotor::BWD) : motor->run(StepperMotor::FWD); - - printf("CAN: setting speed -> %d\n\r", speed); + 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); } } - wait(0.05); + if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,BASE)) + { + if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1) + { + motor->hard_stop(); + motor->set_max_speed(5000); + motor->run(StepperMotor::BWD); + } + } + } } @@ -117,11 +189,10 @@ int main() { can1.frequency(125000); - messageIn.format=CANExtended; - - // Motor Initialization + + // Motor Initialization motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN); - + if (motor->init(&init) != COMPONENT_OK) { printf("ERROR: vvMotor Init\n\r"); @@ -129,36 +200,29 @@ } motor->attach_error_handler(&motor_error_handler); - - // Limit EndStop - end0.rise(&end0_pressed); - end0.fall(&end0_released); - // Zero EndStop - end1.rise(&end1_pressed); + + end0.rise(&end0_int_handler); + end1.rise(&end1_int_handler); end1.fall(&motor_set_home); - - button.rise(&button_int_handler); - + + + motor->set_step_mode(StepperMotor::STEP_MODE_1_16); - - motor_set_home(); - printf("DONE: Motor Init\n\r"); - + // CAN Initialization - + canrxa.start(canrx); - + cantxa.start(cantx); + printf("DONE: CAN Init\n\r"); - + + + printf("Running!\n\r"); - - // DEBUG - //motor->set_max_speed(8000); - //motor->run(StepperMotor::FWD); - + while(true) { - wait(1); + wait(1000); } }