BASE
Dependencies: X-NUCLEO-IHM05A1
Diff: main.cpp
- Revision:
- 26:44175c51a820
- Parent:
- 25:281c8e913db4
- Child:
- 27:275ba9c137c9
--- a/main.cpp Wed Jun 12 16:10:21 2019 +0000 +++ b/main.cpp Tue Jun 18 16:28:38 2019 +0000 @@ -4,6 +4,10 @@ #define VREFA_PWM_PIN D3 #define VREFB_PWM_PIN D9 +#define JOINT_SET_SPEED 20 + +#define JOINT_ID 1 + l6208_init_t init = { 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes @@ -33,18 +37,17 @@ InterruptIn end1(PC_11, PullUp); InterruptIn enc(PC_12, PullUp); -int current_pose = 0; -int pose = 0; +int32_t speed = 0; void motor_error_handler(uint16_t error) { - printf("ERROR: Motor Runtime\n\r"); - while(1){} + printf("ERROR: Motor Runtime\n\r"); + while(1){} } void motor_zero() { - motor->run(StepperMotor::FWD); + motor->run(StepperMotor::FWD); } void button_int_handler() @@ -55,16 +58,15 @@ void end0_int_handler() { - printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); + printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); } void end1_int_handler() { - motor->hard_stop(); + motor->hard_stop(); + motor->run(StepperMotor::BWD); - motor->run(StepperMotor::BWD); - - printf("END1: Pressed\n\r"); + printf("END1: Pressed\n\r"); } void motor_set_home() @@ -72,9 +74,6 @@ motor->hard_stop(); motor->set_home(); motor->go_to(0); - - current_pose = 0; - pose = 0; } // CAN @@ -83,42 +82,25 @@ CANMessage messageIn; CANMessage messageOut; -int filter = can1.filter(0x010, 0x4FF, CANStandard); - void canrx() { while(1) { - if(can1.read(messageIn,filter)&&messageIn.id==0x010) + if(can1.read(messageIn)) { - pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); - printf("CAN: mess %d\n\r", pose); - - - if (pose == 1) - { - motor->run(StepperMotor::FWD); - } - else if (pose == 0) + if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID)) { - motor->soft_stop(); - current_pose= motor->get_position(); - motor->go_to(current_pose); - } - else if (pose == 2) - { - motor->run(StepperMotor::BWD); + 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: mess %d\n\r", speed); } - else - { - motor->soft_stop(); - current_pose= motor->get_position(); - motor->go_to(current_pose); - } - - - // motor->go_to(pose); } + + wait(0.1); } } @@ -127,7 +109,7 @@ int main() { - can1.frequency(125000); + can1.frequency(500000); // Motor Initialization motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN); @@ -155,12 +137,10 @@ printf("DONE: CAN Init\n\r"); - - printf("Running!\n\r"); while(true) { - wait(1000); + wait(1); } }