BASE
Dependencies: X-NUCLEO-IHM05A1
Diff: main.cpp
- Revision:
- 31:5d6a97adae07
- Parent:
- 30:c40b060795a2
- Child:
- 32:465e41868fe4
--- a/main.cpp Wed Sep 04 16:31:22 2019 +0000 +++ b/main.cpp Thu Sep 05 20:16:02 2019 +0000 @@ -1,9 +1,9 @@ #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 @@ -19,21 +19,21 @@ 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, @@ -46,7 +46,7 @@ JOINT_MAXTORQUE, JOINT_ZERO, }CAN_COMMANDS; - + typedef enum { BASE=1, @@ -59,7 +59,7 @@ CAMERA1, CAMERA2, }JOINT; - + float pose, current_pose; float speed, current_speed; void zero() @@ -82,38 +82,40 @@ 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.00000487012987; //do something 0,0004791666667 - angle = (angle - offset) * 100; + 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->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) @@ -128,23 +130,23 @@ 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(60); + osDelay(1/SEND_FREQUENCY*1000); } - - + + void canrx() { while(1) @@ -155,8 +157,8 @@ 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) { motor->set_max_speed(current_speed*80); @@ -167,7 +169,7 @@ motor->set_max_speed(current_speed*80); motor->run(StepperMotor::BWD); } - + else { motor->soft_stop(); @@ -180,9 +182,8 @@ { 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); + zero(); + motor->wait_while_active(); } } @@ -193,9 +194,9 @@ canrx(); osDelay(10); } - + /* Main ----------------------------------------------------------------------*/ - + int main() { @@ -209,28 +210,30 @@ 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 - zero(); + 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