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
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?
User | Revision | Line number | New 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, ¤t_a_setpoint, ¤t_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 | } |