Team DIANA / Mbed OS Arm_stepper_BASE

BASE

Dependencies:   X-NUCLEO-IHM05A1

main.cpp@35:87a2266254e5, 2019-09-13 (annotated)

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