hbjhjb
Dependencies: mbed X-NUCLEO-IHM05A1
Diff: main.cpp
- Revision:
- 33:c1cefad6d338
- Parent:
- 32:465e41868fe4
--- a/main.cpp Fri Sep 13 09:24:19 2019 +0000 +++ b/main.cpp Sun Sep 15 08:47:18 2019 +0000 @@ -1,9 +1,10 @@ #include "mbed.h" #include "L6208.h" - + #define VREFA_PWM_PIN D3 #define VREFB_PWM_PIN D9 -#define SEND_FREQUENCY 10 //Hz +#define BAUDRATE 9600 +#define JOINT 2 l6208_init_t init = { 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes @@ -19,221 +20,134 @@ 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; - +Serial serial(PA_2, PA_3); + + + 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()); + 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) + +void motor_error_handler(uint16_t error) { printf("ERROR: Motor Runtime\n\r"); - + } - + void end1_int_handler() { - // motor->hard_stop(); - + // 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 () + + +void serialrx() { - while(1) + + int id, speed; + + + if(serial.readable() ) { + serial.scanf("%d %d", &id, &speed ); + printf("%d %d\n", id, speed); + if (id==JOINT) + { + led=!led; + current_speed=speed; + } - 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) + + void fmotor() { - // 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); + printf("run FWD\n"); + motor->set_max_speed(abs(current_speed*80)); + motor->run(StepperMotor::BWD); } else if (current_speed<0) { - motor->set_max_speed(current_speed*80); - motor->run(StepperMotor::BWD); + printf("run BWD\n"); + motor->set_max_speed(abs(current_speed*80)); + motor->run(StepperMotor::FWD); } - + else { - motor->soft_stop(); + motor->hard_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() -{ + + + /* 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) + int main() { - 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 + led=1; + serial.baud(BAUDRATE); + + // 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); - canrxa.start(canrx_ISR); - //cantxa.start(cantx_ISR); - - printf("DONE: CAN Init\n\r"); - - - - printf("Running!\n\r"); - - //zero(); - - while(true) - { - wait(1000); - } -} \ No newline at end of file + // end1.rise(&end1_int_handler); + + printf("DONE: Motor Init\n\r"); + + + printf("Running!\n\r"); + + //zero(); + + while(true) + { + serialrx(); + //wait (0.001); + fmotor(); + + } + } \ No newline at end of file