Team DIANA / Mbed OS Arm_stepper_DEVEL

a version to test

Dependencies:   X-NUCLEO-IHM05A1

main.cpp@30:c40b060795a2, 2019-09-04 (annotated)

Committer:
gidiana
Date:
Wed Sep 04 16:31:22 2019 +0000
Revision:
30:c40b060795a2
Parent:
29:f888a2394027 
angles

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