Team DIANA / Mbed OS Arm_stepper_DEVEL

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   X-NUCLEO-IHM05A1

main.cpp@29:f888a2394027, 2019-08-07 (annotated)

Committer:
gidiana
Date:
Wed Aug 07 12:30:41 2019 +0000
Revision:
29:f888a2394027
Parent:
28:8878dd50b7e1
Child:
30:c40b060795a2
new version

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
stebonicelli 25:281c8e913db4 23 Thread canrxa;
gidiana 29:f888a2394027 24 Thread cantxa;
stebonicelli 25:281c8e913db4 25
stebonicelli 25:281c8e913db4 26 // Utility
stebonicelli 25:281c8e913db4 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 29:f888a2394027 33 InterruptIn end0(PA_5, PullUp);
gidiana 29:f888a2394027 34 InterruptIn end1(PA_6, 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 29:f888a2394027 63 long int pose, current_pose;
gidiana 29:f888a2394027 64 int speed, current_speed;
gidiana 29:f888a2394027 65
gidiana 29:f888a2394027 66 uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
stebonicelli 21:533d014f09e0 67 {
gidiana 29:f888a2394027 68 uint32_t id = (uint32_t)can_id; // LSB byte is the controller id.
gidiana 29:f888a2394027 69 id |= (uint32_t)message_id << 8; // Next lowest byte is the packet id.
gidiana 29:f888a2394027 70 id |= 0x80000000; // Send in Extended Frame Format.
gidiana 29:f888a2394027 71 return id;
stebonicelli 21:533d014f09e0 72 }
stebonicelli 21:533d014f09e0 73
gidiana 29:f888a2394027 74 int angle_deparse (long int pose, float offset)
nucleosam 0:36aa6787d4f9 75 {
gidiana 29:f888a2394027 76 offset = offset * 0.00872664625;
gidiana 29:f888a2394027 77 float angle = pose * 0.00000425 ; //do something
gidiana 29:f888a2394027 78 angle = (angle - offset) * 100;
gidiana 29:f888a2394027 79 return (int)angle;
stebonicelli 17:dc1b04f0b55d 80 }
gidiana 29:f888a2394027 81 void motor_error_handler(uint16_t error)
stebonicelli 17:dc1b04f0b55d 82 {
gidiana 29:f888a2394027 83 printf("ERROR: Motor Runtime\n\r");
gidiana 29:f888a2394027 84
nucleosam 0:36aa6787d4f9 85 }
nucleosam 0:36aa6787d4f9 86
gidiana 29:f888a2394027 87 void end0_int_handler()
stebonicelli 28:8878dd50b7e1 88 {
gidiana 29:f888a2394027 89 motor->hard_stop();
gidiana 29:f888a2394027 90
gidiana 29:f888a2394027 91 motor->run(StepperMotor::FWD);
gidiana 29:f888a2394027 92
gidiana 29:f888a2394027 93 printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
gidiana 29:f888a2394027 94
gidiana 29:f888a2394027 95
stebonicelli 28:8878dd50b7e1 96 }
stebonicelli 28:8878dd50b7e1 97
gidiana 29:f888a2394027 98 void end1_int_handler()
stebonicelli 17:dc1b04f0b55d 99 {
gidiana 29:f888a2394027 100 motor->hard_stop();
stebonicelli 17:dc1b04f0b55d 101
gidiana 29:f888a2394027 102 motor->run(StepperMotor::BWD);
gidiana 29:f888a2394027 103
gidiana 29:f888a2394027 104 printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
stebonicelli 17:dc1b04f0b55d 105 }
stebonicelli 17:dc1b04f0b55d 106
stebonicelli 25:281c8e913db4 107 void motor_set_home()
stebonicelli 25:281c8e913db4 108 {
stebonicelli 25:281c8e913db4 109 motor->hard_stop();
stebonicelli 25:281c8e913db4 110 motor->set_home();
stebonicelli 25:281c8e913db4 111 motor->go_to(0);
gidiana 29:f888a2394027 112
stebonicelli 25:281c8e913db4 113 }
stebonicelli 17:dc1b04f0b55d 114
stebonicelli 25:281c8e913db4 115 // CAN
gidiana 27:275ba9c137c9 116 CAN can1(PB_8, PB_9); // RX, TX
stebonicelli 17:dc1b04f0b55d 117
stebonicelli 17:dc1b04f0b55d 118 CANMessage messageIn;
stebonicelli 17:dc1b04f0b55d 119 CANMessage messageOut;
stebonicelli 17:dc1b04f0b55d 120
gidiana 29:f888a2394027 121 void cantx ()
gidiana 29:f888a2394027 122 {
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 29:f888a2394027 126
gidiana 29:f888a2394027 127 messageOut.data[3]=pose;
gidiana 29:f888a2394027 128 messageOut.data[2]=pose >>8;
gidiana 29:f888a2394027 129 messageOut.data[1]=pose >>16;
gidiana 29:f888a2394027 130 messageOut.data[0]=pose >>24;
gidiana 29:f888a2394027 131
gidiana 29:f888a2394027 132 int status = can1.write(messageOut);
gidiana 29:f888a2394027 133 printf("CAN send CURRENT POSITION Joint status %d : pose %d",status, pose);
gidiana 29:f888a2394027 134
gidiana 29:f888a2394027 135 wait(1);
gidiana 29:f888a2394027 136
gidiana 29:f888a2394027 137
gidiana 29:f888a2394027 138
gidiana 29:f888a2394027 139 }
gidiana 29:f888a2394027 140
gidiana 29:f888a2394027 141
gidiana 29:f888a2394027 142
gidiana 18:65707db67191 143 void canrx()
stebonicelli 17:dc1b04f0b55d 144 {
stebonicelli 19:9680ebe86f4a 145 while(1)
gidiana 29:f888a2394027 146 {
gidiana 29:f888a2394027 147 if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE))
stebonicelli 19:9680ebe86f4a 148 {
gidiana 29:f888a2394027 149 speed=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
gidiana 29:f888a2394027 150 printf("CAN: mess %d\n\r", speed);
gidiana 29:f888a2394027 151 current_speed=speed-100;
gidiana 29:f888a2394027 152
gidiana 29:f888a2394027 153
gidiana 29:f888a2394027 154 if (current_speed>0)
stebonicelli 21:533d014f09e0 155 {
gidiana 29:f888a2394027 156 motor->set_max_speed(current_speed*80);
gidiana 29:f888a2394027 157 motor->run(StepperMotor::FWD);
gidiana 29:f888a2394027 158 }
gidiana 29:f888a2394027 159 else if (current_speed<0)
gidiana 29:f888a2394027 160 {
gidiana 29:f888a2394027 161 motor->set_max_speed(current_speed*80);
gidiana 29:f888a2394027 162 motor->run(StepperMotor::BWD);
gidiana 29:f888a2394027 163 }
gidiana 29:f888a2394027 164
gidiana 29:f888a2394027 165 else
gidiana 29:f888a2394027 166 {
gidiana 29:f888a2394027 167 motor->soft_stop();
gidiana 29:f888a2394027 168 current_pose= motor->get_position();
gidiana 29:f888a2394027 169 motor->go_to(current_pose);
stebonicelli 21:533d014f09e0 170 }
stebonicelli 25:281c8e913db4 171 }
stebonicelli 26:44175c51a820 172
gidiana 29:f888a2394027 173 if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,BASE))
gidiana 29:f888a2394027 174 {
gidiana 29:f888a2394027 175 if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
gidiana 29:f888a2394027 176 {
gidiana 29:f888a2394027 177 motor->hard_stop();
gidiana 29:f888a2394027 178 motor->set_max_speed(5000);
gidiana 29:f888a2394027 179 motor->run(StepperMotor::BWD);
gidiana 29:f888a2394027 180 }
gidiana 29:f888a2394027 181 }
gidiana 29:f888a2394027 182
stebonicelli 25:281c8e913db4 183 }
nucleosam 0:36aa6787d4f9 184 }
stebonicelli 25:281c8e913db4 185
stebonicelli 17:dc1b04f0b55d 186
nucleosam 0:36aa6787d4f9 187 /* Main ----------------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 188
nucleosam 0:36aa6787d4f9 189 int main()
nucleosam 0:36aa6787d4f9 190 {
gidiana 27:275ba9c137c9 191 can1.frequency(125000);
gidiana 29:f888a2394027 192
gidiana 29:f888a2394027 193 // Motor Initialization
stebonicelli 25:281c8e913db4 194 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
gidiana 29:f888a2394027 195
stebonicelli 25:281c8e913db4 196 if (motor->init(&init) != COMPONENT_OK)
stebonicelli 17:dc1b04f0b55d 197 {
gidiana 18:65707db67191 198 printf("ERROR: vvMotor Init\n\r");
davide.aliprandi@st.com 5:bc710d77d801 199 exit(EXIT_FAILURE);
davide.aliprandi@st.com 5:bc710d77d801 200 }
nucleosam 0:36aa6787d4f9 201
stebonicelli 25:281c8e913db4 202 motor->attach_error_handler(&motor_error_handler);
gidiana 29:f888a2394027 203
gidiana 29:f888a2394027 204 end0.rise(&end0_int_handler);
gidiana 29:f888a2394027 205 end1.rise(&end1_int_handler);
stebonicelli 25:281c8e913db4 206 end1.fall(&motor_set_home);
gidiana 29:f888a2394027 207
gidiana 29:f888a2394027 208
gidiana 29:f888a2394027 209
stebonicelli 25:281c8e913db4 210 motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
stebonicelli 19:9680ebe86f4a 211 printf("DONE: Motor Init\n\r");
gidiana 29:f888a2394027 212
stebonicelli 17:dc1b04f0b55d 213 // CAN Initialization
gidiana 29:f888a2394027 214
stebonicelli 19:9680ebe86f4a 215 canrxa.start(canrx);
gidiana 29:f888a2394027 216 cantxa.start(cantx);
gidiana 29:f888a2394027 217
stebonicelli 17:dc1b04f0b55d 218 printf("DONE: CAN Init\n\r");
gidiana 29:f888a2394027 219
gidiana 29:f888a2394027 220
gidiana 29:f888a2394027 221
stebonicelli 17:dc1b04f0b55d 222 printf("Running!\n\r");
gidiana 29:f888a2394027 223
stebonicelli 17:dc1b04f0b55d 224 while(true)
gidiana 13:08617f604d55 225 {
gidiana 29:f888a2394027 226 wait(1000);
stebonicelli 17:dc1b04f0b55d 227 }
nucleosam 0:36aa6787d4f9 228 }