Joris Kaal / Mbed 2 deprecated haptic_hid_1s_PRBS

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@0:f3cf9865b7be, 2015-01-16 (annotated)

Committer:
tomlankhorst
Date:
Fri Jan 16 10:47:17 2015 +0000
Revision:
0:f3cf9865b7be
Child:
1:24b7ab90081a
Published

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"
tomlankhorst 0:f3cf9865b7be 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 */
tomlankhorst 0:f3cf9865b7be 16 int main()
tomlankhorst 0:f3cf9865b7be 17 {
tomlankhorst 0:f3cf9865b7be 18 // Initialize system
tomlankhorst 0:f3cf9865b7be 19 initialize_io();
tomlankhorst 0:f3cf9865b7be 20 calibrate_current_sensor();
tomlankhorst 0:f3cf9865b7be 21 calibrate_position();
tomlankhorst 0:f3cf9865b7be 22
tomlankhorst 0:f3cf9865b7be 23 torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US);
tomlankhorst 0:f3cf9865b7be 24
tomlankhorst 0:f3cf9865b7be 25 send_report.length = 16;
tomlankhorst 0:f3cf9865b7be 26 recv_report.length = 16;
tomlankhorst 0:f3cf9865b7be 27
tomlankhorst 0:f3cf9865b7be 28 while(1){
tomlankhorst 0:f3cf9865b7be 29
tomlankhorst 0:f3cf9865b7be 30 int32_t abspos = ABSPOS();
tomlankhorst 0:f3cf9865b7be 31
tomlankhorst 0:f3cf9865b7be 32 send_report.data[3] = abspos & 0x000000ff;
tomlankhorst 0:f3cf9865b7be 33 send_report.data[2] = (abspos & 0x0000ff00) >> 8;
tomlankhorst 0:f3cf9865b7be 34 send_report.data[1] = (abspos & 0x00ff0000) >> 16;
tomlankhorst 0:f3cf9865b7be 35 send_report.data[0] = (abspos & 0xff000000) >> 24;
tomlankhorst 0:f3cf9865b7be 36
tomlankhorst 0:f3cf9865b7be 37 for(int i = 4; i < 16; i++){
tomlankhorst 0:f3cf9865b7be 38 send_report.data[i] = 0x0;
tomlankhorst 0:f3cf9865b7be 39 }
tomlankhorst 0:f3cf9865b7be 40
tomlankhorst 0:f3cf9865b7be 41 //Send the report
tomlankhorst 0:f3cf9865b7be 42 hid.send(&send_report);
tomlankhorst 0:f3cf9865b7be 43
tomlankhorst 0:f3cf9865b7be 44 // Try to read
tomlankhorst 0:f3cf9865b7be 45 if(hid.readNB(&recv_report)) {
tomlankhorst 0:f3cf9865b7be 46
tomlankhorst 0:f3cf9865b7be 47 ZControl_I = (float)1e-6*((recv_report.data[3] << 24) | (recv_report.data[2] << 16) | (recv_report.data[1] << 8) | (recv_report.data[0]));
tomlankhorst 0:f3cf9865b7be 48 ZControl_B = (float)1e-6*((recv_report.data[7] << 24) | (recv_report.data[6] << 16) | (recv_report.data[5] << 8) | (recv_report.data[4]));
tomlankhorst 0:f3cf9865b7be 49 ZControl_K = (float)1e-6*((recv_report.data[11] << 24) | (recv_report.data[10] << 16) | (recv_report.data[9] << 8) | (recv_report.data[8]));
tomlankhorst 0:f3cf9865b7be 50 ZControl_RefPos = (recv_report.data[15] << 24) | (recv_report.data[14] << 16) | (recv_report.data[13] << 8) | (recv_report.data[12]);
tomlankhorst 0:f3cf9865b7be 51 }
tomlankhorst 0:f3cf9865b7be 52
tomlankhorst 0:f3cf9865b7be 53 info_led_3 = !info_led_3;
tomlankhorst 0:f3cf9865b7be 54 wait(0.01);
tomlankhorst 0:f3cf9865b7be 55 }
tomlankhorst 0:f3cf9865b7be 56
tomlankhorst 0:f3cf9865b7be 57 return 0;
tomlankhorst 0:f3cf9865b7be 58 }
tomlankhorst 0:f3cf9865b7be 59
tomlankhorst 0:f3cf9865b7be 60 /** Sample the current sensor to determine the offset
tomlankhorst 0:f3cf9865b7be 61 */
tomlankhorst 0:f3cf9865b7be 62 void calibrate_current_sensor()
tomlankhorst 0:f3cf9865b7be 63 {
tomlankhorst 0:f3cf9865b7be 64 driver_enable_a = 0;
tomlankhorst 0:f3cf9865b7be 65 driver_enable_b = 0;
tomlankhorst 0:f3cf9865b7be 66 for(int i=0; i<100; i++) {
tomlankhorst 0:f3cf9865b7be 67 current_sensor_a_offset+= 0.01f*current_sensor_a.read();
tomlankhorst 0:f3cf9865b7be 68 current_sensor_b_offset+= 0.01f*current_sensor_b.read();
tomlankhorst 0:f3cf9865b7be 69 wait_us(2000);
tomlankhorst 0:f3cf9865b7be 70 }
tomlankhorst 0:f3cf9865b7be 71 driver_enable_a = 1;
tomlankhorst 0:f3cf9865b7be 72 driver_enable_b = 1;
tomlankhorst 0:f3cf9865b7be 73 }
tomlankhorst 0:f3cf9865b7be 74
tomlankhorst 0:f3cf9865b7be 75 /** Calibrates to find the reference position
tomlankhorst 0:f3cf9865b7be 76 */
tomlankhorst 0:f3cf9865b7be 77 void calibrate_position()
tomlankhorst 0:f3cf9865b7be 78 {
tomlankhorst 0:f3cf9865b7be 79 position_ref = 0;
tomlankhorst 0:f3cf9865b7be 80 driver_vref_ab = 0.5;
tomlankhorst 0:f3cf9865b7be 81 for(int i = 0; i < 10; i++) {
tomlankhorst 0:f3cf9865b7be 82 driver_1a = 0.7;
tomlankhorst 0:f3cf9865b7be 83 driver_2a = 0;
tomlankhorst 0:f3cf9865b7be 84 driver_1b = 0;
tomlankhorst 0:f3cf9865b7be 85 driver_2b = 0;
tomlankhorst 0:f3cf9865b7be 86 wait(0.2);
tomlankhorst 0:f3cf9865b7be 87 position_ref+= GET_POSITION();
tomlankhorst 0:f3cf9865b7be 88 driver_1a = 0;
tomlankhorst 0:f3cf9865b7be 89 driver_2a = 0;
tomlankhorst 0:f3cf9865b7be 90 driver_1b = 0.7;
tomlankhorst 0:f3cf9865b7be 91 driver_2b = 0;
tomlankhorst 0:f3cf9865b7be 92 wait(0.01);
tomlankhorst 0:f3cf9865b7be 93 }
tomlankhorst 0:f3cf9865b7be 94 driver_vref_ab = 1;
tomlankhorst 0:f3cf9865b7be 95 position_ref = position_ref/10;
tomlankhorst 0:f3cf9865b7be 96 driver_1b = 0;
tomlankhorst 0:f3cf9865b7be 97 }
tomlankhorst 0:f3cf9865b7be 98
tomlankhorst 0:f3cf9865b7be 99 /** Initialize I/O (PWM, DigitalIn/Out, AnalogIn)
tomlankhorst 0:f3cf9865b7be 100 */
tomlankhorst 0:f3cf9865b7be 101 void initialize_io()
tomlankhorst 0:f3cf9865b7be 102 {
tomlankhorst 0:f3cf9865b7be 103 user_btn.mode(PullUp);
tomlankhorst 0:f3cf9865b7be 104 pc.baud(115200);
tomlankhorst 0:f3cf9865b7be 105 spi.format(14,3);
tomlankhorst 0:f3cf9865b7be 106 driver_1a.period_us(33);
tomlankhorst 0:f3cf9865b7be 107 driver_2a.period_us(33);
tomlankhorst 0:f3cf9865b7be 108 driver_1b.period_us(33);
tomlankhorst 0:f3cf9865b7be 109 driver_2b.period_us(33);
tomlankhorst 0:f3cf9865b7be 110 driver_enable_a = 1;
tomlankhorst 0:f3cf9865b7be 111 driver_enable_b = 1;
tomlankhorst 0:f3cf9865b7be 112 driver_vref_ab = 1;
tomlankhorst 0:f3cf9865b7be 113 }
tomlankhorst 0:f3cf9865b7be 114
tomlankhorst 0:f3cf9865b7be 115 /** Torque Controller function, controls the plant
tomlankhorst 0:f3cf9865b7be 116 * This function is called on an interrupt basis by a Ticker object.
tomlankhorst 0:f3cf9865b7be 117 * PI current controller and a Park transform for FOC
tomlankhorst 0:f3cf9865b7be 118 */
tomlankhorst 0:f3cf9865b7be 119 void torque_control()
tomlankhorst 0:f3cf9865b7be 120 {
tomlankhorst 0:f3cf9865b7be 121
tomlankhorst 0:f3cf9865b7be 122 // Get position
tomlankhorst 0:f3cf9865b7be 123 static int last_position = 0;
tomlankhorst 0:f3cf9865b7be 124 static float last_speed = 0;
tomlankhorst 0:f3cf9865b7be 125 static float position_sin;
tomlankhorst 0:f3cf9865b7be 126 static float position_cos;
tomlankhorst 0:f3cf9865b7be 127 static float position_theta;
tomlankhorst 0:f3cf9865b7be 128 static float torque_setpoint;
tomlankhorst 0:f3cf9865b7be 129 static int position_int;
tomlankhorst 0:f3cf9865b7be 130 position = GET_POSITION();
tomlankhorst 0:f3cf9865b7be 131 #if ENABLE_POSITION_COMPENSATION == 1
tomlankhorst 0:f3cf9865b7be 132 position += position_sensor_error[position];
tomlankhorst 0:f3cf9865b7be 133 #endif
tomlankhorst 0:f3cf9865b7be 134 // Antialias
tomlankhorst 0:f3cf9865b7be 135 if(position - last_position > POSITION_ANTIALIAS) {
tomlankhorst 0:f3cf9865b7be 136 position_offset_count--;
tomlankhorst 0:f3cf9865b7be 137 last_position+=8192;
tomlankhorst 0:f3cf9865b7be 138 }
tomlankhorst 0:f3cf9865b7be 139 if(position - last_position < -POSITION_ANTIALIAS) {
tomlankhorst 0:f3cf9865b7be 140 position_offset_count++;
tomlankhorst 0:f3cf9865b7be 141 last_position-=8192;
tomlankhorst 0:f3cf9865b7be 142 }
tomlankhorst 0:f3cf9865b7be 143
tomlankhorst 0:f3cf9865b7be 144 // Speed and position processing
tomlankhorst 0:f3cf9865b7be 145 static speedEstimator speed_estimator(position);
tomlankhorst 0:f3cf9865b7be 146 speed = 0.00076699f*speed_estimator.get(position+POSITION_RESOLUTION*position_offset_count); // rad/s
tomlankhorst 0:f3cf9865b7be 147 LOWPASSIIR(acceleration, TORQUE_CONTROLLER_INTERVAL_INV*(speed - last_speed), 0.005f);
tomlankhorst 0:f3cf9865b7be 148 last_position = position;
tomlankhorst 0:f3cf9865b7be 149 last_speed = speed;
tomlankhorst 0:f3cf9865b7be 150 position_theta = fmod(1.0f*(position-position_ref+8192),163.84f);
tomlankhorst 0:f3cf9865b7be 151 position_int = floor(position_theta);
tomlankhorst 0:f3cf9865b7be 152 position_theta *= ELECTRICAL_POSITION_TO_RAD;
tomlankhorst 0:f3cf9865b7be 153 position_sin = arm_sin_f32(position_theta);
tomlankhorst 0:f3cf9865b7be 154 position_cos = arm_cos_f32(position_theta);
tomlankhorst 0:f3cf9865b7be 155
tomlankhorst 0:f3cf9865b7be 156 // Impedance controller...
tomlankhorst 0:f3cf9865b7be 157 torque = -ZControl_K*0.00076699f*(ABSPOS()-ZControl_RefPos) - ZControl_B*speed - ZControl_I*acceleration;
tomlankhorst 0:f3cf9865b7be 158
tomlankhorst 0:f3cf9865b7be 159 // Preprocess torque command
tomlankhorst 0:f3cf9865b7be 160 torque_setpoint = (torque > TORQUE_LIMIT) ? TORQUE_LIMIT : (torque < -TORQUE_LIMIT ? -TORQUE_LIMIT : torque);
tomlankhorst 0:f3cf9865b7be 161 #if ENABLE_COGGING_COMPENSATION == 1
tomlankhorst 0:f3cf9865b7be 162 torque_setpoint+= CC_GAIN*(cogging_compensation[position_int]);
tomlankhorst 0:f3cf9865b7be 163 #endif
tomlankhorst 0:f3cf9865b7be 164
tomlankhorst 0:f3cf9865b7be 165 /**
tomlankhorst 0:f3cf9865b7be 166 *F| / Stribeck + Coulomb + Viscous
tomlankhorst 0:f3cf9865b7be 167 * |\_/
tomlankhorst 0:f3cf9865b7be 168 * |____v_ */
tomlankhorst 0:f3cf9865b7be 169
tomlankhorst 0:f3cf9865b7be 170 #if ENABLE_FRICTION_COMPENSATION == 1
tomlankhorst 0:f3cf9865b7be 171 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 172 #endif
tomlankhorst 0:f3cf9865b7be 173 #if ENABLE_DITHER == 1
tomlankhorst 0:f3cf9865b7be 174 dither_tick++;
tomlankhorst 0:f3cf9865b7be 175 if(dither_tick > DITHER_TICKS) {
tomlankhorst 0:f3cf9865b7be 176 dither_tick = 0;
tomlankhorst 0:f3cf9865b7be 177 } else {
tomlankhorst 0:f3cf9865b7be 178 torque_setpoint+=DITHER_FORCE*sin(2*PI/DITHER_TICKS*dither_tick);
tomlankhorst 0:f3cf9865b7be 179 }
tomlankhorst 0:f3cf9865b7be 180 #endif
tomlankhorst 0:f3cf9865b7be 181
tomlankhorst 0:f3cf9865b7be 182 // Transform torque command
tomlankhorst 0:f3cf9865b7be 183 static float current_a_setpoint;
tomlankhorst 0:f3cf9865b7be 184 static float current_b_setpoint;
tomlankhorst 0:f3cf9865b7be 185 arm_inv_park_f32(0, torque_setpoint, &current_a_setpoint, &current_b_setpoint, position_sin, position_cos);
tomlankhorst 0:f3cf9865b7be 186
tomlankhorst 0:f3cf9865b7be 187 // PI Controller
tomlankhorst 0:f3cf9865b7be 188 static float current_a_error;
tomlankhorst 0:f3cf9865b7be 189 static float current_b_error;
tomlankhorst 0:f3cf9865b7be 190 static float current_a_int_error = 0;
tomlankhorst 0:f3cf9865b7be 191 static float current_b_int_error = 0;
tomlankhorst 0:f3cf9865b7be 192 current_a_error = current_a_setpoint - GET_CURRENT_A();
tomlankhorst 0:f3cf9865b7be 193 current_b_error = current_b_setpoint - GET_CURRENT_B();
tomlankhorst 0:f3cf9865b7be 194
tomlankhorst 0:f3cf9865b7be 195 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 196 current_a_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_a_error;
tomlankhorst 0:f3cf9865b7be 197 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 198 current_b_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_b_error;
tomlankhorst 0:f3cf9865b7be 199
tomlankhorst 0:f3cf9865b7be 200 current_a_error *= CURRENT_CONTROLLER_KP;
tomlankhorst 0:f3cf9865b7be 201 current_b_error *= CURRENT_CONTROLLER_KP;
tomlankhorst 0:f3cf9865b7be 202 current_a_error += CURRENT_CONTROLLER_KI*current_a_int_error;
tomlankhorst 0:f3cf9865b7be 203 current_b_error += CURRENT_CONTROLLER_KI*current_b_int_error;
tomlankhorst 0:f3cf9865b7be 204
tomlankhorst 0:f3cf9865b7be 205 // Apply voltages
tomlankhorst 0:f3cf9865b7be 206 driver_1a = current_a_error > 0 ? current_a_error : 0;
tomlankhorst 0:f3cf9865b7be 207 driver_2a = current_a_error < 0 ? -current_a_error : 0;
tomlankhorst 0:f3cf9865b7be 208 driver_1b = current_b_error > 0 ? current_b_error : 0;
tomlankhorst 0:f3cf9865b7be 209 driver_2b = current_b_error < 0 ? -current_b_error : 0;
tomlankhorst 0:f3cf9865b7be 210 }