Team DIANA / Mbed 2 deprecated Arm_stepper_BASE_serial

hbjhjb

Dependencies:   mbed X-NUCLEO-IHM05A1

main.cpp@31:5d6a97adae07, 2019-09-05 (annotated)

Committer:
gidiana
Date:
Thu Sep 05 20:16:02 2019 +0000
Revision:
31:5d6a97adae07
Parent:
30:c40b060795a2
Child:
32:465e41868fe4
improvements

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"
gidiana 31:5d6a97adae07 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 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 31:5d6a97adae07 22
gidiana 30:c40b060795a2 23 Thread cantxa(osPriorityNormal);
gidiana 30:c40b060795a2 24 Thread canrxa(osPriorityNormal);
gidiana 31:5d6a97adae07 25
stebonicelli 25:281c8e913db4 26 // Utility
gidiana 30:c40b060795a2 27 //InterruptIn button(USER_BUTTON);
stebonicelli 25:281c8e913db4 28 DigitalOut led(LED1);
gidiana 31:5d6a97adae07 29
stebonicelli 25:281c8e913db4 30 // Motor Control
stebonicelli 25:281c8e913db4 31 L6208 *motor;
gidiana 31:5d6a97adae07 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);
gidiana 31:5d6a97adae07 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;
gidiana 31:5d6a97adae07 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;
gidiana 31:5d6a97adae07 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 }
gidiana 31:5d6a97adae07 85
gidiana 31:5d6a97adae07 86 double to_rad(double angle)
gidiana 31:5d6a97adae07 87 {
gidiana 31:5d6a97adae07 88 return angle*0.0174533;
gidiana 31:5d6a97adae07 89 }
gidiana 30:c40b060795a2 90 double angle_deparse (long int pose, float offset)
nucleosam 0:36aa6787d4f9 91 {
gidiana 29:f888a2394027 92 offset = offset * 0.00872664625;
gidiana 31:5d6a97adae07 93 double angle = pose *0.000487012987; //do something 0,0004791666667
gidiana 31:5d6a97adae07 94 angle = (angle - offset);
gidiana 30:c40b060795a2 95 return angle;
stebonicelli 17:dc1b04f0b55d 96 }
gidiana 31:5d6a97adae07 97 void motor_error_handler(uint16_t error)
stebonicelli 17:dc1b04f0b55d 98 {
gidiana 29:f888a2394027 99 printf("ERROR: Motor Runtime\n\r");
gidiana 29:f888a2394027 100
nucleosam 0:36aa6787d4f9 101 }
gidiana 31:5d6a97adae07 102
gidiana 29:f888a2394027 103 void end1_int_handler()
stebonicelli 17:dc1b04f0b55d 104 {
gidiana 30:c40b060795a2 105 // motor->hard_stop();
gidiana 31:5d6a97adae07 106
gidiana 30:c40b060795a2 107 motor->run(StepperMotor::FWD);
gidiana 31:5d6a97adae07 108
gidiana 29:f888a2394027 109 printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
stebonicelli 17:dc1b04f0b55d 110 }
gidiana 31:5d6a97adae07 111
gidiana 31:5d6a97adae07 112
stebonicelli 25:281c8e913db4 113 // CAN
gidiana 27:275ba9c137c9 114 CAN can1(PB_8, PB_9); // RX, TX
gidiana 31:5d6a97adae07 115
stebonicelli 17:dc1b04f0b55d 116 CANMessage messageIn;
stebonicelli 17:dc1b04f0b55d 117 CANMessage messageOut;
gidiana 31:5d6a97adae07 118
gidiana 29:f888a2394027 119 void cantx ()
gidiana 29:f888a2394027 120 {
gidiana 30:c40b060795a2 121 while(1)
gidiana 30:c40b060795a2 122 {
gidiana 30:c40b060795a2 123
gidiana 30:c40b060795a2 124 int _pose;
gidiana 29:f888a2394027 125 messageOut.format = CANExtended;
gidiana 29:f888a2394027 126 messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, BASE);
gidiana 29:f888a2394027 127 pose=angle_deparse(motor->get_position(), 0);
gidiana 30:c40b060795a2 128 _pose=pose*100;
gidiana 30:c40b060795a2 129 messageOut.data[3]=_pose;
gidiana 30:c40b060795a2 130 messageOut.data[2]=_pose >>8;
gidiana 30:c40b060795a2 131 messageOut.data[1]=_pose >>16;
gidiana 30:c40b060795a2 132 messageOut.data[0]=_pose >>24;
gidiana 31:5d6a97adae07 133
gidiana 29:f888a2394027 134 int status = can1.write(messageOut);
gidiana 30:c40b060795a2 135 led=!status;
gidiana 30:c40b060795a2 136 printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
gidiana 30:c40b060795a2 137 }
gidiana 30:c40b060795a2 138
gidiana 31:5d6a97adae07 139
gidiana 31:5d6a97adae07 140
gidiana 31:5d6a97adae07 141
gidiana 29:f888a2394027 142 }
gidiana 30:c40b060795a2 143 void cantx_ISR()
gidiana 30:c40b060795a2 144 {
gidiana 30:c40b060795a2 145 cantx();
gidiana 31:5d6a97adae07 146 osDelay(1/SEND_FREQUENCY*1000);
gidiana 30:c40b060795a2 147 }
gidiana 31:5d6a97adae07 148
gidiana 31:5d6a97adae07 149
gidiana 18:65707db67191 150 void canrx()
stebonicelli 17:dc1b04f0b55d 151 {
stebonicelli 19:9680ebe86f4a 152 while(1)
gidiana 29:f888a2394027 153 {
gidiana 30:c40b060795a2 154 // printf("receive\t\n");
gidiana 29:f888a2394027 155 if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE))
stebonicelli 19:9680ebe86f4a 156 {
gidiana 29:f888a2394027 157 speed=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
gidiana 29:f888a2394027 158 printf("CAN: mess %d\n\r", speed);
gidiana 29:f888a2394027 159 current_speed=speed-100;
gidiana 31:5d6a97adae07 160
gidiana 31:5d6a97adae07 161
gidiana 29:f888a2394027 162 if (current_speed>0)
stebonicelli 21:533d014f09e0 163 {
gidiana 29:f888a2394027 164 motor->set_max_speed(current_speed*80);
gidiana 29:f888a2394027 165 motor->run(StepperMotor::FWD);
gidiana 29:f888a2394027 166 }
gidiana 29:f888a2394027 167 else if (current_speed<0)
gidiana 29:f888a2394027 168 {
gidiana 29:f888a2394027 169 motor->set_max_speed(current_speed*80);
gidiana 29:f888a2394027 170 motor->run(StepperMotor::BWD);
gidiana 29:f888a2394027 171 }
gidiana 31:5d6a97adae07 172
gidiana 29:f888a2394027 173 else
gidiana 29:f888a2394027 174 {
gidiana 29:f888a2394027 175 motor->soft_stop();
gidiana 29:f888a2394027 176 current_pose= motor->get_position();
gidiana 29:f888a2394027 177 motor->go_to(current_pose);
stebonicelli 21:533d014f09e0 178 }
stebonicelli 25:281c8e913db4 179 }
stebonicelli 26:44175c51a820 180
gidiana 29:f888a2394027 181 if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,BASE))
gidiana 29:f888a2394027 182 {
gidiana 29:f888a2394027 183 if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
gidiana 29:f888a2394027 184 {
gidiana 31:5d6a97adae07 185 zero();
gidiana 31:5d6a97adae07 186 motor->wait_while_active();
gidiana 29:f888a2394027 187 }
gidiana 29:f888a2394027 188 }
gidiana 29:f888a2394027 189
stebonicelli 25:281c8e913db4 190 }
nucleosam 0:36aa6787d4f9 191 }
gidiana 30:c40b060795a2 192 void canrx_ISR()
gidiana 30:c40b060795a2 193 {
gidiana 30:c40b060795a2 194 canrx();
gidiana 30:c40b060795a2 195 osDelay(10);
gidiana 30:c40b060795a2 196 }
gidiana 31:5d6a97adae07 197
nucleosam 0:36aa6787d4f9 198 /* Main ----------------------------------------------------------------------*/
gidiana 31:5d6a97adae07 199
nucleosam 0:36aa6787d4f9 200 int main()
nucleosam 0:36aa6787d4f9 201 {
gidiana 30:c40b060795a2 202
gidiana 27:275ba9c137c9 203 can1.frequency(125000);
gidiana 30:c40b060795a2 204
gidiana 29:f888a2394027 205 // Motor Initialization
stebonicelli 25:281c8e913db4 206 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
gidiana 30:c40b060795a2 207 motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
stebonicelli 25:281c8e913db4 208 if (motor->init(&init) != COMPONENT_OK)
stebonicelli 17:dc1b04f0b55d 209 {
gidiana 18:65707db67191 210 printf("ERROR: vvMotor Init\n\r");
davide.aliprandi@st.com 5:bc710d77d801 211 exit(EXIT_FAILURE);
davide.aliprandi@st.com 5:bc710d77d801 212 }
gidiana 31:5d6a97adae07 213
stebonicelli 25:281c8e913db4 214 motor->attach_error_handler(&motor_error_handler);
gidiana 31:5d6a97adae07 215
gidiana 31:5d6a97adae07 216
gidiana 30:c40b060795a2 217 end1.rise(&end1_int_handler);
gidiana 31:5d6a97adae07 218
stebonicelli 19:9680ebe86f4a 219 printf("DONE: Motor Init\n\r");
gidiana 31:5d6a97adae07 220
stebonicelli 17:dc1b04f0b55d 221 // CAN Initialization
gidiana 31:5d6a97adae07 222
gidiana 30:c40b060795a2 223
gidiana 30:c40b060795a2 224 canrxa.start(canrx_ISR);
gidiana 30:c40b060795a2 225 cantxa.start(cantx_ISR);
gidiana 30:c40b060795a2 226
stebonicelli 17:dc1b04f0b55d 227 printf("DONE: CAN Init\n\r");
gidiana 31:5d6a97adae07 228
gidiana 31:5d6a97adae07 229
gidiana 31:5d6a97adae07 230
stebonicelli 17:dc1b04f0b55d 231 printf("Running!\n\r");
gidiana 31:5d6a97adae07 232
gidiana 31:5d6a97adae07 233 zero();
gidiana 31:5d6a97adae07 234
stebonicelli 17:dc1b04f0b55d 235 while(true)
gidiana 13:08617f604d55 236 {
gidiana 29:f888a2394027 237 wait(1000);
stebonicelli 17:dc1b04f0b55d 238 }
gidiana 31:5d6a97adae07 239 }