Team DIANA / Mbed OS Arm_stepper_SHOULDER

SHOULDER

Dependencies:   X-NUCLEO-IHM05A1

main.cpp@32:03c98e297a4a, 2019-09-13 (annotated)

Committer:
gidiana
Date:
Fri Sep 13 09:26:41 2019 +0000
Revision:
32:03c98e297a4a
Parent:
31:f24535e65dae
Child:
33:48196cd5c052
little endian

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