Joris Kaal / Mbed 2 deprecated haptic_hid_beter_metuitgangp2

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:   MODSERIAL USBDevice compensation_tables mbed-dsp mbed

Fork of haptic_hid by First Last

main.cpp@1:24b7ab90081a, 2015-01-17 (annotated)

Committer:
vsluiter
Date:
Sat Jan 17 21:42:46 2015 +0000
Revision:
1:24b7ab90081a
Parent:
0:f3cf9865b7be
Child:
2:bf29d24b69dd
Added state machine for impedances

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tomlankhorst 0:f3cf9865b7be 1 #include "mbed.h"
tomlankhorst 0:f3cf9865b7be 2 #include "arm_math.h"
vsluiter 1:24b7ab90081a 3 //#include "USBHID.h"
tomlankhorst 0:f3cf9865b7be 4 #include <math.h>
tomlankhorst 0:f3cf9865b7be 5 #include <string>
tomlankhorst 0:f3cf9865b7be 6 #include <stdlib.h>
tomlankhorst 0:f3cf9865b7be 7 #include "MODSERIAL.h"
tomlankhorst 0:f3cf9865b7be 8 #include "speedestimator.h"
tomlankhorst 0:f3cf9865b7be 9 #include "position_sensor_error.h"
tomlankhorst 0:f3cf9865b7be 10 #include "cogging_compensation.h"
tomlankhorst 0:f3cf9865b7be 11 #include "main.h"
tomlankhorst 0:f3cf9865b7be 12
tomlankhorst 0:f3cf9865b7be 13 /** Main function
tomlankhorst 0:f3cf9865b7be 14 * Bootstraps the system
tomlankhorst 0:f3cf9865b7be 15 */
vsluiter 1:24b7ab90081a 16 typedef enum z_state{Z_ZERO,Z_B,Z_I,Z_K,Z_OFF}z_states;
vsluiter 1:24b7ab90081a 17
vsluiter 1:24b7ab90081a 18 void SetImpedance(float i, float b, float k, float pos)
vsluiter 1:24b7ab90081a 19 {
vsluiter 1:24b7ab90081a 20 ZControl_I = i;
vsluiter 1:24b7ab90081a 21 ZControl_B = b;
vsluiter 1:24b7ab90081a 22 ZControl_K = k;
vsluiter 1:24b7ab90081a 23 ZControl_RefPos = pos;
vsluiter 1:24b7ab90081a 24 }
vsluiter 1:24b7ab90081a 25
vsluiter 1:24b7ab90081a 26 void blink(void)
vsluiter 1:24b7ab90081a 27 {
vsluiter 1:24b7ab90081a 28 static z_states localstate=Z_ZERO;
vsluiter 1:24b7ab90081a 29 switch(localstate)
vsluiter 1:24b7ab90081a 30 {
vsluiter 1:24b7ab90081a 31 case Z_ZERO:
vsluiter 1:24b7ab90081a 32 {
vsluiter 1:24b7ab90081a 33 localstate = Z_B;
vsluiter 1:24b7ab90081a 34 SetImpedance(0,0.01,0,position);
vsluiter 1:24b7ab90081a 35 break;
vsluiter 1:24b7ab90081a 36 }
vsluiter 1:24b7ab90081a 37 case Z_B:
vsluiter 1:24b7ab90081a 38 {
vsluiter 1:24b7ab90081a 39 localstate = Z_I;
vsluiter 1:24b7ab90081a 40 SetImpedance(0.01,0,0,position);
vsluiter 1:24b7ab90081a 41 break;
vsluiter 1:24b7ab90081a 42 }
vsluiter 1:24b7ab90081a 43 case Z_I:
vsluiter 1:24b7ab90081a 44 {
vsluiter 1:24b7ab90081a 45 localstate = Z_K;
vsluiter 1:24b7ab90081a 46 SetImpedance(0,0,0.01,position);
vsluiter 1:24b7ab90081a 47 break;
vsluiter 1:24b7ab90081a 48 }
vsluiter 1:24b7ab90081a 49 case Z_K:
vsluiter 1:24b7ab90081a 50 {
vsluiter 1:24b7ab90081a 51 localstate = Z_OFF;
vsluiter 1:24b7ab90081a 52 SetImpedance(0,0,0,position);
vsluiter 1:24b7ab90081a 53 driver_enable_a = 0;
vsluiter 1:24b7ab90081a 54 driver_enable_b = 0;
vsluiter 1:24b7ab90081a 55 break;
vsluiter 1:24b7ab90081a 56 }
vsluiter 1:24b7ab90081a 57 case Z_OFF:
vsluiter 1:24b7ab90081a 58 {
vsluiter 1:24b7ab90081a 59 localstate = Z_ZERO;
vsluiter 1:24b7ab90081a 60 SetImpedance(0,0.00,0,position);
vsluiter 1:24b7ab90081a 61 driver_enable_a = 1;
vsluiter 1:24b7ab90081a 62 driver_enable_b = 1;
vsluiter 1:24b7ab90081a 63 break;
vsluiter 1:24b7ab90081a 64 }
vsluiter 1:24b7ab90081a 65 default:
vsluiter 1:24b7ab90081a 66 {
vsluiter 1:24b7ab90081a 67 localstate = Z_ZERO;
vsluiter 1:24b7ab90081a 68 ZControl_I = 0;
vsluiter 1:24b7ab90081a 69 ZControl_B = 0;
vsluiter 1:24b7ab90081a 70 ZControl_K = 0;
vsluiter 1:24b7ab90081a 71 ZControl_RefPos = position;
vsluiter 1:24b7ab90081a 72 }
vsluiter 1:24b7ab90081a 73 }
vsluiter 1:24b7ab90081a 74 info_led_3 != info_led_3;
vsluiter 1:24b7ab90081a 75 wait_ms(300); //debounce
vsluiter 1:24b7ab90081a 76 }
vsluiter 1:24b7ab90081a 77
tomlankhorst 0:f3cf9865b7be 78 int main()
tomlankhorst 0:f3cf9865b7be 79 {
tomlankhorst 0:f3cf9865b7be 80 // Initialize system
tomlankhorst 0:f3cf9865b7be 81 initialize_io();
tomlankhorst 0:f3cf9865b7be 82 calibrate_current_sensor();
tomlankhorst 0:f3cf9865b7be 83 calibrate_position();
tomlankhorst 0:f3cf9865b7be 84
tomlankhorst 0:f3cf9865b7be 85 torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US);
vsluiter 1:24b7ab90081a 86
vsluiter 1:24b7ab90081a 87 //send_report.length = 16;
vsluiter 1:24b7ab90081a 88 //recv_report.length = 16;
vsluiter 1:24b7ab90081a 89
vsluiter 1:24b7ab90081a 90 while(1) {
tomlankhorst 0:f3cf9865b7be 91 int32_t abspos = ABSPOS();
vsluiter 1:24b7ab90081a 92
vsluiter 1:24b7ab90081a 93 if(!user_btn)
vsluiter 1:24b7ab90081a 94 blink();
vsluiter 1:24b7ab90081a 95 //send_report.data[3] = abspos & 0x000000ff;
vsluiter 1:24b7ab90081a 96 //send_report.data[2] = (abspos & 0x0000ff00) >> 8;
vsluiter 1:24b7ab90081a 97 //send_report.data[1] = (abspos & 0x00ff0000) >> 16;
vsluiter 1:24b7ab90081a 98 //send_report.data[0] = (abspos & 0xff000000) >> 24;
vsluiter 1:24b7ab90081a 99
vsluiter 1:24b7ab90081a 100 //for(int i = 4; i < 16; i++){
vsluiter 1:24b7ab90081a 101 // send_report.data[i] = 0x0;
vsluiter 1:24b7ab90081a 102 //}
vsluiter 1:24b7ab90081a 103
tomlankhorst 0:f3cf9865b7be 104 //Send the report
vsluiter 1:24b7ab90081a 105 //hid.send(&send_report);
vsluiter 1:24b7ab90081a 106
tomlankhorst 0:f3cf9865b7be 107 // Try to read
vsluiter 1:24b7ab90081a 108 //if(hid.readNB(&recv_report)) {
vsluiter 1:24b7ab90081a 109
vsluiter 1:24b7ab90081a 110 // ZControl_I = (float)1e-6*((recv_report.data[3] << 24) | (recv_report.data[2] << 16) | (recv_report.data[1] << 8) | (recv_report.data[0]));
vsluiter 1:24b7ab90081a 111 // ZControl_B = (float)1e-6*((recv_report.data[7] << 24) | (recv_report.data[6] << 16) | (recv_report.data[5] << 8) | (recv_report.data[4]));
vsluiter 1:24b7ab90081a 112 // ZControl_K = (float)1e-6*((recv_report.data[11] << 24) | (recv_report.data[10] << 16) | (recv_report.data[9] << 8) | (recv_report.data[8]));
vsluiter 1:24b7ab90081a 113 // ZControl_RefPos = (recv_report.data[15] << 24) | (recv_report.data[14] << 16) | (recv_report.data[13] << 8) | (recv_report.data[12]);
vsluiter 1:24b7ab90081a 114 //}
vsluiter 1:24b7ab90081a 115
tomlankhorst 0:f3cf9865b7be 116 info_led_3 = !info_led_3;
tomlankhorst 0:f3cf9865b7be 117 wait(0.01);
tomlankhorst 0:f3cf9865b7be 118 }
tomlankhorst 0:f3cf9865b7be 119
tomlankhorst 0:f3cf9865b7be 120 return 0;
tomlankhorst 0:f3cf9865b7be 121 }
tomlankhorst 0:f3cf9865b7be 122
tomlankhorst 0:f3cf9865b7be 123 /** Sample the current sensor to determine the offset
tomlankhorst 0:f3cf9865b7be 124 */
tomlankhorst 0:f3cf9865b7be 125 void calibrate_current_sensor()
tomlankhorst 0:f3cf9865b7be 126 {
tomlankhorst 0:f3cf9865b7be 127 driver_enable_a = 0;
tomlankhorst 0:f3cf9865b7be 128 driver_enable_b = 0;
tomlankhorst 0:f3cf9865b7be 129 for(int i=0; i<100; i++) {
tomlankhorst 0:f3cf9865b7be 130 current_sensor_a_offset+= 0.01f*current_sensor_a.read();
tomlankhorst 0:f3cf9865b7be 131 current_sensor_b_offset+= 0.01f*current_sensor_b.read();
tomlankhorst 0:f3cf9865b7be 132 wait_us(2000);
tomlankhorst 0:f3cf9865b7be 133 }
tomlankhorst 0:f3cf9865b7be 134 driver_enable_a = 1;
tomlankhorst 0:f3cf9865b7be 135 driver_enable_b = 1;
tomlankhorst 0:f3cf9865b7be 136 }
tomlankhorst 0:f3cf9865b7be 137
tomlankhorst 0:f3cf9865b7be 138 /** Calibrates to find the reference position
tomlankhorst 0:f3cf9865b7be 139 */
tomlankhorst 0:f3cf9865b7be 140 void calibrate_position()
tomlankhorst 0:f3cf9865b7be 141 {
tomlankhorst 0:f3cf9865b7be 142 position_ref = 0;
tomlankhorst 0:f3cf9865b7be 143 driver_vref_ab = 0.5;
tomlankhorst 0:f3cf9865b7be 144 for(int i = 0; i < 10; i++) {
tomlankhorst 0:f3cf9865b7be 145 driver_1a = 0.7;
tomlankhorst 0:f3cf9865b7be 146 driver_2a = 0;
tomlankhorst 0:f3cf9865b7be 147 driver_1b = 0;
tomlankhorst 0:f3cf9865b7be 148 driver_2b = 0;
tomlankhorst 0:f3cf9865b7be 149 wait(0.2);
tomlankhorst 0:f3cf9865b7be 150 position_ref+= GET_POSITION();
tomlankhorst 0:f3cf9865b7be 151 driver_1a = 0;
tomlankhorst 0:f3cf9865b7be 152 driver_2a = 0;
tomlankhorst 0:f3cf9865b7be 153 driver_1b = 0.7;
tomlankhorst 0:f3cf9865b7be 154 driver_2b = 0;
tomlankhorst 0:f3cf9865b7be 155 wait(0.01);
tomlankhorst 0:f3cf9865b7be 156 }
tomlankhorst 0:f3cf9865b7be 157 driver_vref_ab = 1;
tomlankhorst 0:f3cf9865b7be 158 position_ref = position_ref/10;
tomlankhorst 0:f3cf9865b7be 159 driver_1b = 0;
tomlankhorst 0:f3cf9865b7be 160 }
tomlankhorst 0:f3cf9865b7be 161
tomlankhorst 0:f3cf9865b7be 162 /** Initialize I/O (PWM, DigitalIn/Out, AnalogIn)
tomlankhorst 0:f3cf9865b7be 163 */
tomlankhorst 0:f3cf9865b7be 164 void initialize_io()
tomlankhorst 0:f3cf9865b7be 165 {
tomlankhorst 0:f3cf9865b7be 166 user_btn.mode(PullUp);
vsluiter 1:24b7ab90081a 167 //user_btn.rise(blink);
tomlankhorst 0:f3cf9865b7be 168 pc.baud(115200);
tomlankhorst 0:f3cf9865b7be 169 spi.format(14,3);
tomlankhorst 0:f3cf9865b7be 170 driver_1a.period_us(33);
tomlankhorst 0:f3cf9865b7be 171 driver_2a.period_us(33);
tomlankhorst 0:f3cf9865b7be 172 driver_1b.period_us(33);
tomlankhorst 0:f3cf9865b7be 173 driver_2b.period_us(33);
tomlankhorst 0:f3cf9865b7be 174 driver_enable_a = 1;
tomlankhorst 0:f3cf9865b7be 175 driver_enable_b = 1;
tomlankhorst 0:f3cf9865b7be 176 driver_vref_ab = 1;
tomlankhorst 0:f3cf9865b7be 177 }
tomlankhorst 0:f3cf9865b7be 178
tomlankhorst 0:f3cf9865b7be 179 /** Torque Controller function, controls the plant
tomlankhorst 0:f3cf9865b7be 180 * This function is called on an interrupt basis by a Ticker object.
tomlankhorst 0:f3cf9865b7be 181 * PI current controller and a Park transform for FOC
tomlankhorst 0:f3cf9865b7be 182 */
tomlankhorst 0:f3cf9865b7be 183 void torque_control()
tomlankhorst 0:f3cf9865b7be 184 {
tomlankhorst 0:f3cf9865b7be 185
tomlankhorst 0:f3cf9865b7be 186 // Get position
tomlankhorst 0:f3cf9865b7be 187 static int last_position = 0;
tomlankhorst 0:f3cf9865b7be 188 static float last_speed = 0;
tomlankhorst 0:f3cf9865b7be 189 static float position_sin;
tomlankhorst 0:f3cf9865b7be 190 static float position_cos;
tomlankhorst 0:f3cf9865b7be 191 static float position_theta;
tomlankhorst 0:f3cf9865b7be 192 static float torque_setpoint;
tomlankhorst 0:f3cf9865b7be 193 static int position_int;
tomlankhorst 0:f3cf9865b7be 194 position = GET_POSITION();
tomlankhorst 0:f3cf9865b7be 195 #if ENABLE_POSITION_COMPENSATION == 1
tomlankhorst 0:f3cf9865b7be 196 position += position_sensor_error[position];
tomlankhorst 0:f3cf9865b7be 197 #endif
tomlankhorst 0:f3cf9865b7be 198 // Antialias
tomlankhorst 0:f3cf9865b7be 199 if(position - last_position > POSITION_ANTIALIAS) {
tomlankhorst 0:f3cf9865b7be 200 position_offset_count--;
tomlankhorst 0:f3cf9865b7be 201 last_position+=8192;
tomlankhorst 0:f3cf9865b7be 202 }
tomlankhorst 0:f3cf9865b7be 203 if(position - last_position < -POSITION_ANTIALIAS) {
tomlankhorst 0:f3cf9865b7be 204 position_offset_count++;
tomlankhorst 0:f3cf9865b7be 205 last_position-=8192;
tomlankhorst 0:f3cf9865b7be 206 }
tomlankhorst 0:f3cf9865b7be 207
tomlankhorst 0:f3cf9865b7be 208 // Speed and position processing
tomlankhorst 0:f3cf9865b7be 209 static speedEstimator speed_estimator(position);
tomlankhorst 0:f3cf9865b7be 210 speed = 0.00076699f*speed_estimator.get(position+POSITION_RESOLUTION*position_offset_count); // rad/s
tomlankhorst 0:f3cf9865b7be 211 LOWPASSIIR(acceleration, TORQUE_CONTROLLER_INTERVAL_INV*(speed - last_speed), 0.005f);
tomlankhorst 0:f3cf9865b7be 212 last_position = position;
tomlankhorst 0:f3cf9865b7be 213 last_speed = speed;
tomlankhorst 0:f3cf9865b7be 214 position_theta = fmod(1.0f*(position-position_ref+8192),163.84f);
tomlankhorst 0:f3cf9865b7be 215 position_int = floor(position_theta);
tomlankhorst 0:f3cf9865b7be 216 position_theta *= ELECTRICAL_POSITION_TO_RAD;
tomlankhorst 0:f3cf9865b7be 217 position_sin = arm_sin_f32(position_theta);
tomlankhorst 0:f3cf9865b7be 218 position_cos = arm_cos_f32(position_theta);
tomlankhorst 0:f3cf9865b7be 219
tomlankhorst 0:f3cf9865b7be 220 // Impedance controller...
tomlankhorst 0:f3cf9865b7be 221 torque = -ZControl_K*0.00076699f*(ABSPOS()-ZControl_RefPos) - ZControl_B*speed - ZControl_I*acceleration;
tomlankhorst 0:f3cf9865b7be 222
tomlankhorst 0:f3cf9865b7be 223 // Preprocess torque command
tomlankhorst 0:f3cf9865b7be 224 torque_setpoint = (torque > TORQUE_LIMIT) ? TORQUE_LIMIT : (torque < -TORQUE_LIMIT ? -TORQUE_LIMIT : torque);
tomlankhorst 0:f3cf9865b7be 225 #if ENABLE_COGGING_COMPENSATION == 1
tomlankhorst 0:f3cf9865b7be 226 torque_setpoint+= CC_GAIN*(cogging_compensation[position_int]);
tomlankhorst 0:f3cf9865b7be 227 #endif
tomlankhorst 0:f3cf9865b7be 228
tomlankhorst 0:f3cf9865b7be 229 /**
tomlankhorst 0:f3cf9865b7be 230 *F| / Stribeck + Coulomb + Viscous
tomlankhorst 0:f3cf9865b7be 231 * |\_/
tomlankhorst 0:f3cf9865b7be 232 * |____v_ */
tomlankhorst 0:f3cf9865b7be 233
tomlankhorst 0:f3cf9865b7be 234 #if ENABLE_FRICTION_COMPENSATION == 1
tomlankhorst 0:f3cf9865b7be 235 torque_setpoint+= tanh(COULOMB_VELOCITY_CONST*speed) * (COULOMB_FRICTION + (STRIBECK_FRICTION-COULOMB_FRICTION)*exp(-abs(speed/STRIBECK_VELOCITY_CONST))) + (speed > 0 ? VISCOUS_FRICTION_COEF_FWD : VISCOUS_FRICTION_COEF_REV)*speed;
tomlankhorst 0:f3cf9865b7be 236 #endif
tomlankhorst 0:f3cf9865b7be 237 #if ENABLE_DITHER == 1
tomlankhorst 0:f3cf9865b7be 238 dither_tick++;
tomlankhorst 0:f3cf9865b7be 239 if(dither_tick > DITHER_TICKS) {
tomlankhorst 0:f3cf9865b7be 240 dither_tick = 0;
tomlankhorst 0:f3cf9865b7be 241 } else {
tomlankhorst 0:f3cf9865b7be 242 torque_setpoint+=DITHER_FORCE*sin(2*PI/DITHER_TICKS*dither_tick);
tomlankhorst 0:f3cf9865b7be 243 }
tomlankhorst 0:f3cf9865b7be 244 #endif
tomlankhorst 0:f3cf9865b7be 245
tomlankhorst 0:f3cf9865b7be 246 // Transform torque command
tomlankhorst 0:f3cf9865b7be 247 static float current_a_setpoint;
tomlankhorst 0:f3cf9865b7be 248 static float current_b_setpoint;
tomlankhorst 0:f3cf9865b7be 249 arm_inv_park_f32(0, torque_setpoint, &current_a_setpoint, &current_b_setpoint, position_sin, position_cos);
tomlankhorst 0:f3cf9865b7be 250
tomlankhorst 0:f3cf9865b7be 251 // PI Controller
tomlankhorst 0:f3cf9865b7be 252 static float current_a_error;
tomlankhorst 0:f3cf9865b7be 253 static float current_b_error;
tomlankhorst 0:f3cf9865b7be 254 static float current_a_int_error = 0;
tomlankhorst 0:f3cf9865b7be 255 static float current_b_int_error = 0;
tomlankhorst 0:f3cf9865b7be 256 current_a_error = current_a_setpoint - GET_CURRENT_A();
tomlankhorst 0:f3cf9865b7be 257 current_b_error = current_b_setpoint - GET_CURRENT_B();
tomlankhorst 0:f3cf9865b7be 258
tomlankhorst 0:f3cf9865b7be 259 if(!(current_a_int_error > CURRENT_CONTROLLER_I_LIMIT && current_a_error > 0) && !(current_a_int_error < -CURRENT_CONTROLLER_I_LIMIT && current_a_error < 0))
tomlankhorst 0:f3cf9865b7be 260 current_a_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_a_error;
tomlankhorst 0:f3cf9865b7be 261 if(!(current_b_int_error > CURRENT_CONTROLLER_I_LIMIT && current_b_error > 0) && !(current_b_int_error < -CURRENT_CONTROLLER_I_LIMIT && current_b_error < 0))
tomlankhorst 0:f3cf9865b7be 262 current_b_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_b_error;
tomlankhorst 0:f3cf9865b7be 263
tomlankhorst 0:f3cf9865b7be 264 current_a_error *= CURRENT_CONTROLLER_KP;
tomlankhorst 0:f3cf9865b7be 265 current_b_error *= CURRENT_CONTROLLER_KP;
tomlankhorst 0:f3cf9865b7be 266 current_a_error += CURRENT_CONTROLLER_KI*current_a_int_error;
tomlankhorst 0:f3cf9865b7be 267 current_b_error += CURRENT_CONTROLLER_KI*current_b_int_error;
tomlankhorst 0:f3cf9865b7be 268
tomlankhorst 0:f3cf9865b7be 269 // Apply voltages
tomlankhorst 0:f3cf9865b7be 270 driver_1a = current_a_error > 0 ? current_a_error : 0;
tomlankhorst 0:f3cf9865b7be 271 driver_2a = current_a_error < 0 ? -current_a_error : 0;
tomlankhorst 0:f3cf9865b7be 272 driver_1b = current_b_error > 0 ? current_b_error : 0;
tomlankhorst 0:f3cf9865b7be 273 driver_2b = current_b_error < 0 ? -current_b_error : 0;
tomlankhorst 0:f3cf9865b7be 274 }