Team DIANA / Mbed OS Arm_stepper_SHOULDER

SHOULDER

Dependencies:   X-NUCLEO-IHM05A1

main.cpp@34:4be6353f0186, 2019-09-13 (annotated)

Committer:
stebonicelli
Date:
Fri Sep 13 20:40:50 2019 +0000
Revision:
34:4be6353f0186
Parent:
33:48196cd5c052 
Post dinner revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stebonicelli 17:dc1b04f0b55d 1 #include "mbed.h"
stebonicelli 25:281c8e913db4 2 #include "L6208.h"
stebonicelli 25:281c8e913db4 3
stebonicelli 25:281c8e913db4 4 #define VREFA_PWM_PIN D3
stebonicelli 25:281c8e913db4 5 #define VREFB_PWM_PIN D9
gidiana 31:f24535e65dae 6 #define SEND_FREQUENCY 10 //Hz
stebonicelli 34:4be6353f0186 7
stebonicelli 25:281c8e913db4 8 l6208_init_t init =
stebonicelli 25:281c8e913db4 9 {
stebonicelli 34:4be6353f0186 10 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
stebonicelli 34:4be6353f0186 11 80, //Acceleration current torque in % (from 0 to 100)
stebonicelli 34:4be6353f0186 12 8000, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
stebonicelli 34:4be6353f0186 13 80, //Deceleration current torque in % (from 0 to 100)
stebonicelli 34:4be6353f0186 14 8000, //Running speed in step/s or (1/16)th step/s for microstep modes
stebonicelli 34:4be6353f0186 15 80, //Running current torque in % (from 0 to 100)
stebonicelli 34:4be6353f0186 16 40, //Holding current torque in % (from 0 to 100)
stebonicelli 34:4be6353f0186 17 STEP_MODE_1_16, //Step mode via enum motorStepMode_t
stebonicelli 34:4be6353f0186 18 FAST_DECAY, //Decay mode via enum motorDecayMode_t
stebonicelli 34:4be6353f0186 19 0, //Dwelling time in ms
stebonicelli 34:4be6353f0186 20 FALSE, //Automatic HIZ STOP
stebonicelli 34:4be6353f0186 21 100000 //VREFA and VREFB PWM frequency (Hz)
stebonicelli 25:281c8e913db4 22 };
stebonicelli 34:4be6353f0186 23
gidiana 32:03c98e297a4a 24 // CAN
gidiana 32:03c98e297a4a 25 CAN can1(PA_11, PA_12); // RX, TX
gidiana 32:03c98e297a4a 26
gidiana 32:03c98e297a4a 27 CANMessage messageIn;
gidiana 32:03c98e297a4a 28 CANMessage messageOut;
nucleosam 0:36aa6787d4f9 29
gidiana 31:f24535e65dae 30 Thread cantxa(osPriorityNormal);
gidiana 31:f24535e65dae 31 Thread canrxa(osPriorityNormal);
stebonicelli 25:281c8e913db4 32
stebonicelli 25:281c8e913db4 33 // Utility
gidiana 31:f24535e65dae 34 //InterruptIn button(USER_BUTTON);
stebonicelli 25:281c8e913db4 35 DigitalOut led(LED1);
stebonicelli 25:281c8e913db4 36
stebonicelli 25:281c8e913db4 37 // Motor Control
stebonicelli 25:281c8e913db4 38 L6208 *motor;
stebonicelli 25:281c8e913db4 39
gidiana 29:f888a2394027 40 InterruptIn end0(PA_5, PullUp);
gidiana 29:f888a2394027 41 InterruptIn end1(PA_6, PullUp);
gidiana 32:03c98e297a4a 42 //InterruptIn enc(PC_12, PullUp);
stebonicelli 17:dc1b04f0b55d 43
gidiana 29:f888a2394027 44 typedef enum
gidiana 29:f888a2394027 45 {
stebonicelli 34:4be6353f0186 46 JOINT_SET_SPEED = 20,
stebonicelli 34:4be6353f0186 47 JOINT_SET_POSITION,
stebonicelli 34:4be6353f0186 48 JOINT_CURRENT_POSITION,
stebonicelli 34:4be6353f0186 49 JOINT_CURRENT_SPEED,
stebonicelli 34:4be6353f0186 50 JOINT_STATUS,
stebonicelli 34:4be6353f0186 51 JOINT_ERROR,
stebonicelli 34:4be6353f0186 52 JOINT_TORQUE,
stebonicelli 34:4be6353f0186 53 JOINT_MAXTORQUE,
stebonicelli 34:4be6353f0186 54 JOINT_ZERO,
gidiana 29:f888a2394027 55 }CAN_COMMANDS;
nucleosam 0:36aa6787d4f9 56
gidiana 29:f888a2394027 57 typedef enum
stebonicelli 17:dc1b04f0b55d 58 {
stebonicelli 34:4be6353f0186 59 BASE=1,
stebonicelli 34:4be6353f0186 60 SHOULDER,
stebonicelli 34:4be6353f0186 61 ELBOW,
stebonicelli 34:4be6353f0186 62 WRIST1,
stebonicelli 34:4be6353f0186 63 WRIST2,
stebonicelli 34:4be6353f0186 64 WRIST3,
stebonicelli 34:4be6353f0186 65 END_EFFECTOR,
stebonicelli 34:4be6353f0186 66 CAMERA1,
stebonicelli 34:4be6353f0186 67 CAMERA2,
gidiana 29:f888a2394027 68 }JOINT;
stebonicelli 17:dc1b04f0b55d 69
gidiana 31:f24535e65dae 70 float pose, current_pose;
gidiana 31:f24535e65dae 71 float speed, current_speed;
gidiana 29:f888a2394027 72
gidiana 31:f24535e65dae 73 void set_zero()
gidiana 31:f24535e65dae 74 {
stebonicelli 34:4be6353f0186 75 printf("set zero\t\n");
stebonicelli 34:4be6353f0186 76
stebonicelli 34:4be6353f0186 77 motor->hard_stop();
stebonicelli 34:4be6353f0186 78 motor->set_home();
stebonicelli 34:4be6353f0186 79 motor->go_to(0);
gidiana 31:f24535e65dae 80 }
stebonicelli 34:4be6353f0186 81
gidiana 31:f24535e65dae 82 void zero()
gidiana 31:f24535e65dae 83 {
gidiana 31:f24535e65dae 84 printf("zero");
gidiana 32:03c98e297a4a 85 motor->run(StepperMotor::FWD);
gidiana 31:f24535e65dae 86 }
stebonicelli 34:4be6353f0186 87
gidiana 29:f888a2394027 88 uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
stebonicelli 21:533d014f09e0 89 {
stebonicelli 34:4be6353f0186 90 uint32_t id = (uint32_t)can_id; // LSB byte is the controller id.
stebonicelli 34:4be6353f0186 91 id |= (uint32_t)message_id << 8; // Next lowest byte is the packet id.
stebonicelli 34:4be6353f0186 92 id |= 0x80000000; // Send in Extended Frame Format.
stebonicelli 34:4be6353f0186 93 return id;
stebonicelli 21:533d014f09e0 94 }
stebonicelli 34:4be6353f0186 95
gidiana 31:f24535e65dae 96 double to_rad(double angle)
gidiana 31:f24535e65dae 97 {
stebonicelli 34:4be6353f0186 98 return angle*0.0174533;
gidiana 31:f24535e65dae 99 }
stebonicelli 34:4be6353f0186 100
gidiana 31:f24535e65dae 101 double angle_deparse (long int pose, float offset)
nucleosam 0:36aa6787d4f9 102 {
stebonicelli 34:4be6353f0186 103 offset = offset * 0.00872664625;
stebonicelli 34:4be6353f0186 104 double angle = pose *0.000487012987; //do something 0,0004791666667
stebonicelli 34:4be6353f0186 105 angle = (angle - offset);
stebonicelli 34:4be6353f0186 106 return angle;
stebonicelli 17:dc1b04f0b55d 107 }
stebonicelli 34:4be6353f0186 108
gidiana 29:f888a2394027 109 void motor_error_handler(uint16_t error)
stebonicelli 17:dc1b04f0b55d 110 {
stebonicelli 34:4be6353f0186 111 printf("ERROR: Motor Runtime\n\r");
stebonicelli 34:4be6353f0186 112 }
gidiana 29:f888a2394027 113
gidiana 29:f888a2394027 114 void end0_int_handler()
stebonicelli 28:8878dd50b7e1 115 {
stebonicelli 34:4be6353f0186 116 motor->hard_stop();
stebonicelli 34:4be6353f0186 117 motor->run(StepperMotor::BWD);
stebonicelli 34:4be6353f0186 118
stebonicelli 34:4be6353f0186 119 printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
stebonicelli 34:4be6353f0186 120 }
gidiana 29:f888a2394027 121
gidiana 32:03c98e297a4a 122 void end0_int_handler_fall()
gidiana 32:03c98e297a4a 123 {
stebonicelli 34:4be6353f0186 124 motor->hard_stop();
stebonicelli 34:4be6353f0186 125 int position = motor->get_position();
stebonicelli 34:4be6353f0186 126 motor->go_to(position);
gidiana 32:03c98e297a4a 127 }
stebonicelli 28:8878dd50b7e1 128
gidiana 29:f888a2394027 129 void end1_int_handler()
stebonicelli 17:dc1b04f0b55d 130 {
stebonicelli 34:4be6353f0186 131 motor->hard_stop();
stebonicelli 34:4be6353f0186 132 motor->run(StepperMotor::FWD);
stebonicelli 34:4be6353f0186 133
stebonicelli 34:4be6353f0186 134 printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
stebonicelli 17:dc1b04f0b55d 135 }
stebonicelli 17:dc1b04f0b55d 136
gidiana 29:f888a2394027 137 void cantx ()
gidiana 29:f888a2394027 138 {
gidiana 31:f24535e65dae 139 while(1)
gidiana 31:f24535e65dae 140 {
stebonicelli 34:4be6353f0186 141 int _pose;
stebonicelli 34:4be6353f0186 142
stebonicelli 34:4be6353f0186 143 messageOut.format = CANExtended;
stebonicelli 34:4be6353f0186 144 messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, SHOULDER);
stebonicelli 34:4be6353f0186 145
stebonicelli 34:4be6353f0186 146 pose=angle_deparse(motor->get_position(), 0);
stebonicelli 34:4be6353f0186 147 _pose=pose*100;
stebonicelli 34:4be6353f0186 148
stebonicelli 34:4be6353f0186 149 messageOut.data[3]=_pose;
stebonicelli 34:4be6353f0186 150 messageOut.data[2]=_pose >>8;
stebonicelli 34:4be6353f0186 151 messageOut.data[1]=_pose >>16;
stebonicelli 34:4be6353f0186 152 messageOut.data[0]=_pose >>24;
stebonicelli 34:4be6353f0186 153
stebonicelli 34:4be6353f0186 154 int status = can1.write(messageOut);
stebonicelli 34:4be6353f0186 155 printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
gidiana 31:f24535e65dae 156 }
gidiana 29:f888a2394027 157 }
stebonicelli 34:4be6353f0186 158
gidiana 31:f24535e65dae 159 void cantx_ISR()
gidiana 31:f24535e65dae 160 {
gidiana 31:f24535e65dae 161 cantx();
gidiana 31:f24535e65dae 162 osDelay(1/SEND_FREQUENCY*1000);
gidiana 31:f24535e65dae 163 }
gidiana 29:f888a2394027 164
gidiana 18:65707db67191 165 void canrx()
stebonicelli 17:dc1b04f0b55d 166 {
stebonicelli 34:4be6353f0186 167 while(1)
stebonicelli 34:4be6353f0186 168 {
stebonicelli 34:4be6353f0186 169 if(can1.read(messageIn))
stebonicelli 34:4be6353f0186 170 {
stebonicelli 34:4be6353f0186 171 led = !led;
stebonicelli 34:4be6353f0186 172
stebonicelli 34:4be6353f0186 173 printf("Read!\n\r");
stebonicelli 34:4be6353f0186 174
stebonicelli 34:4be6353f0186 175 if(messageIn.id==gen_can_id(JOINT_SET_SPEED,SHOULDER))
stebonicelli 34:4be6353f0186 176 {
stebonicelli 34:4be6353f0186 177 speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
stebonicelli 34:4be6353f0186 178 printf("CAN: mess received : %f\n\r", speed);
stebonicelli 34:4be6353f0186 179 current_speed=speed;
stebonicelli 34:4be6353f0186 180
stebonicelli 34:4be6353f0186 181 if (current_speed>0)
stebonicelli 34:4be6353f0186 182 {
stebonicelli 34:4be6353f0186 183 motor->set_max_speed(current_speed*80);
stebonicelli 34:4be6353f0186 184 motor->run(StepperMotor::FWD);
stebonicelli 34:4be6353f0186 185 }
stebonicelli 34:4be6353f0186 186 else if (current_speed<0)
stebonicelli 34:4be6353f0186 187 {
stebonicelli 34:4be6353f0186 188 motor->set_max_speed(current_speed*80);
stebonicelli 34:4be6353f0186 189 motor->run(StepperMotor::BWD);
stebonicelli 34:4be6353f0186 190 }
stebonicelli 34:4be6353f0186 191 else
stebonicelli 34:4be6353f0186 192 {
stebonicelli 34:4be6353f0186 193 motor->soft_stop();
stebonicelli 34:4be6353f0186 194 current_pose= motor->get_position();
stebonicelli 34:4be6353f0186 195 motor->go_to(current_pose);
stebonicelli 34:4be6353f0186 196 }
stebonicelli 34:4be6353f0186 197 }
stebonicelli 34:4be6353f0186 198 else if(messageIn.id==gen_can_id(JOINT_ZERO,SHOULDER))
stebonicelli 34:4be6353f0186 199 {
stebonicelli 34:4be6353f0186 200 if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1)
stebonicelli 34:4be6353f0186 201 {
stebonicelli 34:4be6353f0186 202 zero();
stebonicelli 34:4be6353f0186 203 motor->wait_while_active();
stebonicelli 34:4be6353f0186 204 }
stebonicelli 34:4be6353f0186 205 }
stebonicelli 34:4be6353f0186 206 }
stebonicelli 34:4be6353f0186 207 }
stebonicelli 34:4be6353f0186 208 }
gidiana 29:f888a2394027 209
gidiana 31:f24535e65dae 210 void canrx_ISR()
gidiana 31:f24535e65dae 211 {
gidiana 31:f24535e65dae 212 canrx();
gidiana 31:f24535e65dae 213 osDelay(10);
gidiana 31:f24535e65dae 214 }
stebonicelli 17:dc1b04f0b55d 215
nucleosam 0:36aa6787d4f9 216 /* Main ----------------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 217
nucleosam 0:36aa6787d4f9 218 int main()
nucleosam 0:36aa6787d4f9 219 {
stebonicelli 34:4be6353f0186 220
stebonicelli 34:4be6353f0186 221 can1.frequency(125000);
stebonicelli 34:4be6353f0186 222
stebonicelli 34:4be6353f0186 223 // Motor Initialization
stebonicelli 34:4be6353f0186 224 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
stebonicelli 34:4be6353f0186 225 motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
stebonicelli 34:4be6353f0186 226
stebonicelli 34:4be6353f0186 227 if (motor->init(&init) != COMPONENT_OK)
stebonicelli 34:4be6353f0186 228 {
stebonicelli 34:4be6353f0186 229 printf("ERROR: vvMotor Init\n\r");
stebonicelli 34:4be6353f0186 230 exit(EXIT_FAILURE);
stebonicelli 34:4be6353f0186 231 }
stebonicelli 34:4be6353f0186 232
stebonicelli 34:4be6353f0186 233 motor->attach_error_handler(&motor_error_handler);
stebonicelli 34:4be6353f0186 234
stebonicelli 34:4be6353f0186 235 end0.rise(&end0_int_handler);
stebonicelli 34:4be6353f0186 236 end0.fall(&end0_int_handler_fall);
stebonicelli 34:4be6353f0186 237 end1.rise(&end1_int_handler);
stebonicelli 34:4be6353f0186 238 end1.fall(&set_zero);
gidiana 32:03c98e297a4a 239
stebonicelli 34:4be6353f0186 240 printf("DONE: Motor Init\n\r");
stebonicelli 34:4be6353f0186 241
stebonicelli 34:4be6353f0186 242 // CAN Initialization
stebonicelli 34:4be6353f0186 243 // zero();
stebonicelli 34:4be6353f0186 244
stebonicelli 34:4be6353f0186 245 canrxa.start(canrx_ISR);
stebonicelli 34:4be6353f0186 246 //cantxa.start(cantx_ISR);
stebonicelli 34:4be6353f0186 247
stebonicelli 34:4be6353f0186 248 printf("DONE: CAN Init\n\r");
stebonicelli 34:4be6353f0186 249
stebonicelli 34:4be6353f0186 250 printf("Running!\n\r");
stebonicelli 34:4be6353f0186 251
stebonicelli 34:4be6353f0186 252 //zero();
stebonicelli 34:4be6353f0186 253
stebonicelli 34:4be6353f0186 254 while(true)
stebonicelli 34:4be6353f0186 255 {
stebonicelli 34:4be6353f0186 256 wait(1000);
stebonicelli 34:4be6353f0186 257 }
nucleosam 0:36aa6787d4f9 258 }