Joris Kaal / Mbed 2 deprecated haptic_hid_1s_PRBS

PRBS signal on Haptic_hid with output signal of position

Dependencies:   MODSERIAL USBDevice compensation_tables mbed-dsp mbed

Fork of haptic_hid by First Last

main.cpp@3:10863117020c, 2017-06-19 (annotated)

Committer:
JKaal
Date:
Mon Jun 19 07:40:22 2017 +0000
Revision:
3:10863117020c
Parent:
2:bf29d24b69dd
Child:
4:9d37f163d64c
PRBS signal on haptic_hid with output signal

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"
JKaal 3:10863117020c 12 Ticker tickObject;
JKaal 3:10863117020c 13
JKaal 3:10863117020c 14 #include <iostream>
JKaal 3:10863117020c 15
JKaal 3:10863117020c 16 using namespace std;
JKaal 3:10863117020c 17 const int n = 1000;
JKaal 3:10863117020c 18 int k = 0;
JKaal 3:10863117020c 19 int z = 0;
JKaal 3:10863117020c 20 int prbstest[n];
JKaal 3:10863117020c 21 int status;
JKaal 3:10863117020c 22 int statusnew;
JKaal 3:10863117020c 23 const int p = 2*n;
JKaal 3:10863117020c 24 int positievec[p];
JKaal 3:10863117020c 25 int q = 0;
tomlankhorst 0:f3cf9865b7be 26
tomlankhorst 0:f3cf9865b7be 27 /** Main function
tomlankhorst 0:f3cf9865b7be 28 * Bootstraps the system
tomlankhorst 0:f3cf9865b7be 29 */
JKaal 3:10863117020c 30 typedef enum z_state{/*Z_ZERO,Z_B,Z_K,Z_OFF*/Z_I,Z_NUL}z_states;
vsluiter 1:24b7ab90081a 31
vsluiter 1:24b7ab90081a 32 void SetImpedance(float i, float b, float k, float pos)
vsluiter 1:24b7ab90081a 33 {
vsluiter 1:24b7ab90081a 34 ZControl_I = i;
vsluiter 1:24b7ab90081a 35 ZControl_B = b;
vsluiter 1:24b7ab90081a 36 ZControl_K = k;
vsluiter 1:24b7ab90081a 37 ZControl_RefPos = pos;
vsluiter 1:24b7ab90081a 38 }
vsluiter 1:24b7ab90081a 39
vsluiter 1:24b7ab90081a 40 void blink(void)
vsluiter 1:24b7ab90081a 41 {
JKaal 3:10863117020c 42 static z_states localstate=Z_I;
vsluiter 1:24b7ab90081a 43 switch(localstate)
vsluiter 1:24b7ab90081a 44 {
JKaal 3:10863117020c 45 /*case Z_ZERO:
vsluiter 1:24b7ab90081a 46 {
vsluiter 2:bf29d24b69dd 47 localstate = Z_B; //mass
vsluiter 2:bf29d24b69dd 48 SetImpedance(0,0.03,0,position);
vsluiter 1:24b7ab90081a 49 break;
vsluiter 1:24b7ab90081a 50 }
vsluiter 1:24b7ab90081a 51 case Z_B:
vsluiter 1:24b7ab90081a 52 {
vsluiter 2:bf29d24b69dd 53 localstate = Z_I; //fluid
vsluiter 2:bf29d24b69dd 54 SetImpedance(0.0009,0.01,0.001,position);
vsluiter 1:24b7ab90081a 55 break;
JKaal 3:10863117020c 56 }*/
vsluiter 1:24b7ab90081a 57 case Z_I:
vsluiter 1:24b7ab90081a 58 {
JKaal 3:10863117020c 59 localstate = Z_NUL;//spring
JKaal 3:10863117020c 60 SetImpedance(0.00033,0.00033,0.0000034,0);
vsluiter 1:24b7ab90081a 61 break;
vsluiter 1:24b7ab90081a 62 }
JKaal 3:10863117020c 63 case Z_NUL:
JKaal 3:10863117020c 64 {
JKaal 3:10863117020c 65 localstate = Z_I;//spring
JKaal 3:10863117020c 66 SetImpedance(0.00033,0.00033,0.0000034,500);
JKaal 3:10863117020c 67 break;
JKaal 3:10863117020c 68 }
JKaal 3:10863117020c 69 /*case Z_K:
vsluiter 1:24b7ab90081a 70 {
vsluiter 1:24b7ab90081a 71 localstate = Z_OFF;
vsluiter 1:24b7ab90081a 72 SetImpedance(0,0,0,position);
vsluiter 1:24b7ab90081a 73 driver_enable_a = 0;
vsluiter 1:24b7ab90081a 74 driver_enable_b = 0;
vsluiter 1:24b7ab90081a 75 break;
vsluiter 1:24b7ab90081a 76 }
vsluiter 1:24b7ab90081a 77 case Z_OFF:
vsluiter 1:24b7ab90081a 78 {
JKaal 3:10863117020c 79 localstate = Z_I;
vsluiter 1:24b7ab90081a 80 SetImpedance(0,0.00,0,position);
vsluiter 1:24b7ab90081a 81 driver_enable_a = 1;
vsluiter 1:24b7ab90081a 82 driver_enable_b = 1;
vsluiter 1:24b7ab90081a 83 break;
JKaal 3:10863117020c 84 }*/
vsluiter 1:24b7ab90081a 85 default:
vsluiter 1:24b7ab90081a 86 {
JKaal 3:10863117020c 87 localstate = Z_I;
vsluiter 1:24b7ab90081a 88 ZControl_I = 0;
vsluiter 1:24b7ab90081a 89 ZControl_B = 0;
vsluiter 1:24b7ab90081a 90 ZControl_K = 0;
vsluiter 1:24b7ab90081a 91 ZControl_RefPos = position;
vsluiter 1:24b7ab90081a 92 }
vsluiter 1:24b7ab90081a 93 }
vsluiter 1:24b7ab90081a 94 info_led_3 != info_led_3;
vsluiter 1:24b7ab90081a 95 wait_ms(300); //debounce
vsluiter 1:24b7ab90081a 96 }
JKaal 3:10863117020c 97
JKaal 3:10863117020c 98 void printer(){
JKaal 3:10863117020c 99 int positie = GET_POSITION();
JKaal 3:10863117020c 100 positievec[q] = positie;
JKaal 3:10863117020c 101 cout << positievec[q] << ",";
JKaal 3:10863117020c 102 q = q+1;
JKaal 3:10863117020c 103 }
JKaal 3:10863117020c 104
tomlankhorst 0:f3cf9865b7be 105 int main()
tomlankhorst 0:f3cf9865b7be 106 {
tomlankhorst 0:f3cf9865b7be 107 // Initialize system
JKaal 3:10863117020c 108 //initialiseer_prbs();
tomlankhorst 0:f3cf9865b7be 109 initialize_io();
tomlankhorst 0:f3cf9865b7be 110 calibrate_current_sensor();
tomlankhorst 0:f3cf9865b7be 111 calibrate_position();
tomlankhorst 0:f3cf9865b7be 112
JKaal 3:10863117020c 113 for (int i = 0; i <= n; i++) {
JKaal 3:10863117020c 114 prbstest[i] = rand() % 2;
JKaal 3:10863117020c 115 cout << prbstest[i] << ",";//"\r\n";
JKaal 3:10863117020c 116 }
JKaal 3:10863117020c 117
JKaal 3:10863117020c 118 while(z<=n){
JKaal 3:10863117020c 119 //torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US);
JKaal 3:10863117020c 120 //tickObject.attach_us(&printer, 100000);
JKaal 3:10863117020c 121 torque_control();
JKaal 3:10863117020c 122 printer();
JKaal 3:10863117020c 123 cout << z << ";\r\n";
JKaal 3:10863117020c 124 z = z+1;
JKaal 3:10863117020c 125 wait_us(TORQUE_CONTROLLER_INTERVAL_US);
JKaal 3:10863117020c 126
JKaal 3:10863117020c 127 }
vsluiter 1:24b7ab90081a 128
vsluiter 1:24b7ab90081a 129 //send_report.length = 16;
vsluiter 1:24b7ab90081a 130 //recv_report.length = 16;
vsluiter 1:24b7ab90081a 131
JKaal 3:10863117020c 132 cout << "\r\n";
JKaal 3:10863117020c 133 /*
JKaal 3:10863117020c 134 for(int k=0; k <= n; k++) {
JKaal 3:10863117020c 135 status = prbstest[k];
JKaal 3:10863117020c 136 int kplus = k+1;
JKaal 3:10863117020c 137 statusnew = prbstest[kplus];
JKaal 3:10863117020c 138 if (status == statusnew){
JKaal 3:10863117020c 139 //cout << "equal:\r\n";
JKaal 3:10863117020c 140 //cout << status << " " << k << "\r\n";
JKaal 3:10863117020c 141 //cout << statusnew << " " << kplus << "\r\n";
JKaal 3:10863117020c 142 }
JKaal 3:10863117020c 143 else if (status != statusnew){
JKaal 3:10863117020c 144 //cout << "unequal:\r\n";
JKaal 3:10863117020c 145 //cout << status << " " << k << "\r\n";
JKaal 3:10863117020c 146 //cout << statusnew << " " << kplus << "\r\n";
JKaal 3:10863117020c 147 blink();
JKaal 3:10863117020c 148 }
JKaal 3:10863117020c 149 }*/
JKaal 3:10863117020c 150 cout << positievec;
JKaal 3:10863117020c 151
vsluiter 1:24b7ab90081a 152 while(1) {
tomlankhorst 0:f3cf9865b7be 153 int32_t abspos = ABSPOS();
JKaal 3:10863117020c 154
JKaal 3:10863117020c 155 /*if(!user_btn)
vsluiter 1:24b7ab90081a 156 blink();
JKaal 3:10863117020c 157 tickObject.attach(&blink, 5);*/
JKaal 3:10863117020c 158
JKaal 3:10863117020c 159 //blink();
JKaal 3:10863117020c 160 //wait(1);
JKaal 3:10863117020c 161
vsluiter 1:24b7ab90081a 162 //send_report.data[3] = abspos & 0x000000ff;
vsluiter 1:24b7ab90081a 163 //send_report.data[2] = (abspos & 0x0000ff00) >> 8;
vsluiter 1:24b7ab90081a 164 //send_report.data[1] = (abspos & 0x00ff0000) >> 16;
vsluiter 1:24b7ab90081a 165 //send_report.data[0] = (abspos & 0xff000000) >> 24;
vsluiter 1:24b7ab90081a 166
vsluiter 1:24b7ab90081a 167 //for(int i = 4; i < 16; i++){
vsluiter 1:24b7ab90081a 168 // send_report.data[i] = 0x0;
vsluiter 1:24b7ab90081a 169 //}
vsluiter 1:24b7ab90081a 170
tomlankhorst 0:f3cf9865b7be 171 //Send the report
vsluiter 1:24b7ab90081a 172 //hid.send(&send_report);
vsluiter 1:24b7ab90081a 173
tomlankhorst 0:f3cf9865b7be 174 // Try to read
vsluiter 1:24b7ab90081a 175 //if(hid.readNB(&recv_report)) {
vsluiter 1:24b7ab90081a 176
vsluiter 1:24b7ab90081a 177 // 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 178 // 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 179 // 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 180 // ZControl_RefPos = (recv_report.data[15] << 24) | (recv_report.data[14] << 16) | (recv_report.data[13] << 8) | (recv_report.data[12]);
vsluiter 1:24b7ab90081a 181 //}
vsluiter 1:24b7ab90081a 182
tomlankhorst 0:f3cf9865b7be 183 info_led_3 = !info_led_3;
tomlankhorst 0:f3cf9865b7be 184 wait(0.01);
tomlankhorst 0:f3cf9865b7be 185 }
tomlankhorst 0:f3cf9865b7be 186 return 0;
tomlankhorst 0:f3cf9865b7be 187 }
tomlankhorst 0:f3cf9865b7be 188
JKaal 3:10863117020c 189 /*//create a prbs signal
JKaal 3:10863117020c 190 void initialiseer_prbs() {
JKaal 3:10863117020c 191 int prbstest[n] = {};
JKaal 3:10863117020c 192
JKaal 3:10863117020c 193 for (int i = 0; i <= n; i++) {
JKaal 3:10863117020c 194 prbstest[i] = rand() % 2;
JKaal 3:10863117020c 195 cout << prbstest[i];
JKaal 3:10863117020c 196 }
JKaal 3:10863117020c 197 }*/
JKaal 3:10863117020c 198
tomlankhorst 0:f3cf9865b7be 199 /** Sample the current sensor to determine the offset
tomlankhorst 0:f3cf9865b7be 200 */
tomlankhorst 0:f3cf9865b7be 201 void calibrate_current_sensor()
tomlankhorst 0:f3cf9865b7be 202 {
tomlankhorst 0:f3cf9865b7be 203 driver_enable_a = 0;
tomlankhorst 0:f3cf9865b7be 204 driver_enable_b = 0;
tomlankhorst 0:f3cf9865b7be 205 for(int i=0; i<100; i++) {
tomlankhorst 0:f3cf9865b7be 206 current_sensor_a_offset+= 0.01f*current_sensor_a.read();
tomlankhorst 0:f3cf9865b7be 207 current_sensor_b_offset+= 0.01f*current_sensor_b.read();
tomlankhorst 0:f3cf9865b7be 208 wait_us(2000);
tomlankhorst 0:f3cf9865b7be 209 }
tomlankhorst 0:f3cf9865b7be 210 driver_enable_a = 1;
tomlankhorst 0:f3cf9865b7be 211 driver_enable_b = 1;
tomlankhorst 0:f3cf9865b7be 212 }
tomlankhorst 0:f3cf9865b7be 213
tomlankhorst 0:f3cf9865b7be 214 /** Calibrates to find the reference position
tomlankhorst 0:f3cf9865b7be 215 */
tomlankhorst 0:f3cf9865b7be 216 void calibrate_position()
tomlankhorst 0:f3cf9865b7be 217 {
tomlankhorst 0:f3cf9865b7be 218 position_ref = 0;
tomlankhorst 0:f3cf9865b7be 219 driver_vref_ab = 0.5;
tomlankhorst 0:f3cf9865b7be 220 for(int i = 0; i < 10; i++) {
tomlankhorst 0:f3cf9865b7be 221 driver_1a = 0.7;
tomlankhorst 0:f3cf9865b7be 222 driver_2a = 0;
tomlankhorst 0:f3cf9865b7be 223 driver_1b = 0;
tomlankhorst 0:f3cf9865b7be 224 driver_2b = 0;
tomlankhorst 0:f3cf9865b7be 225 wait(0.2);
tomlankhorst 0:f3cf9865b7be 226 position_ref+= GET_POSITION();
tomlankhorst 0:f3cf9865b7be 227 driver_1a = 0;
tomlankhorst 0:f3cf9865b7be 228 driver_2a = 0;
tomlankhorst 0:f3cf9865b7be 229 driver_1b = 0.7;
tomlankhorst 0:f3cf9865b7be 230 driver_2b = 0;
tomlankhorst 0:f3cf9865b7be 231 wait(0.01);
tomlankhorst 0:f3cf9865b7be 232 }
tomlankhorst 0:f3cf9865b7be 233 driver_vref_ab = 1;
tomlankhorst 0:f3cf9865b7be 234 position_ref = position_ref/10;
tomlankhorst 0:f3cf9865b7be 235 driver_1b = 0;
tomlankhorst 0:f3cf9865b7be 236 }
tomlankhorst 0:f3cf9865b7be 237
tomlankhorst 0:f3cf9865b7be 238 /** Initialize I/O (PWM, DigitalIn/Out, AnalogIn)
tomlankhorst 0:f3cf9865b7be 239 */
tomlankhorst 0:f3cf9865b7be 240 void initialize_io()
tomlankhorst 0:f3cf9865b7be 241 {
tomlankhorst 0:f3cf9865b7be 242 user_btn.mode(PullUp);
vsluiter 1:24b7ab90081a 243 //user_btn.rise(blink);
JKaal 3:10863117020c 244 pc.baud(9600);
tomlankhorst 0:f3cf9865b7be 245 spi.format(14,3);
tomlankhorst 0:f3cf9865b7be 246 driver_1a.period_us(33);
tomlankhorst 0:f3cf9865b7be 247 driver_2a.period_us(33);
tomlankhorst 0:f3cf9865b7be 248 driver_1b.period_us(33);
tomlankhorst 0:f3cf9865b7be 249 driver_2b.period_us(33);
tomlankhorst 0:f3cf9865b7be 250 driver_enable_a = 1;
tomlankhorst 0:f3cf9865b7be 251 driver_enable_b = 1;
tomlankhorst 0:f3cf9865b7be 252 driver_vref_ab = 1;
tomlankhorst 0:f3cf9865b7be 253 }
tomlankhorst 0:f3cf9865b7be 254
tomlankhorst 0:f3cf9865b7be 255 /** Torque Controller function, controls the plant
tomlankhorst 0:f3cf9865b7be 256 * This function is called on an interrupt basis by a Ticker object.
tomlankhorst 0:f3cf9865b7be 257 * PI current controller and a Park transform for FOC
tomlankhorst 0:f3cf9865b7be 258 */
tomlankhorst 0:f3cf9865b7be 259 void torque_control()
tomlankhorst 0:f3cf9865b7be 260 {
tomlankhorst 0:f3cf9865b7be 261
tomlankhorst 0:f3cf9865b7be 262 // Get position
tomlankhorst 0:f3cf9865b7be 263 static int last_position = 0;
tomlankhorst 0:f3cf9865b7be 264 static float last_speed = 0;
tomlankhorst 0:f3cf9865b7be 265 static float position_sin;
tomlankhorst 0:f3cf9865b7be 266 static float position_cos;
tomlankhorst 0:f3cf9865b7be 267 static float position_theta;
tomlankhorst 0:f3cf9865b7be 268 static float torque_setpoint;
tomlankhorst 0:f3cf9865b7be 269 static int position_int;
tomlankhorst 0:f3cf9865b7be 270 position = GET_POSITION();
tomlankhorst 0:f3cf9865b7be 271 #if ENABLE_POSITION_COMPENSATION == 1
tomlankhorst 0:f3cf9865b7be 272 position += position_sensor_error[position];
tomlankhorst 0:f3cf9865b7be 273 #endif
tomlankhorst 0:f3cf9865b7be 274 // Antialias
tomlankhorst 0:f3cf9865b7be 275 if(position - last_position > POSITION_ANTIALIAS) {
tomlankhorst 0:f3cf9865b7be 276 position_offset_count--;
tomlankhorst 0:f3cf9865b7be 277 last_position+=8192;
tomlankhorst 0:f3cf9865b7be 278 }
tomlankhorst 0:f3cf9865b7be 279 if(position - last_position < -POSITION_ANTIALIAS) {
tomlankhorst 0:f3cf9865b7be 280 position_offset_count++;
tomlankhorst 0:f3cf9865b7be 281 last_position-=8192;
tomlankhorst 0:f3cf9865b7be 282 }
tomlankhorst 0:f3cf9865b7be 283
tomlankhorst 0:f3cf9865b7be 284 // Speed and position processing
tomlankhorst 0:f3cf9865b7be 285 static speedEstimator speed_estimator(position);
tomlankhorst 0:f3cf9865b7be 286 speed = 0.00076699f*speed_estimator.get(position+POSITION_RESOLUTION*position_offset_count); // rad/s
tomlankhorst 0:f3cf9865b7be 287 LOWPASSIIR(acceleration, TORQUE_CONTROLLER_INTERVAL_INV*(speed - last_speed), 0.005f);
tomlankhorst 0:f3cf9865b7be 288 last_position = position;
tomlankhorst 0:f3cf9865b7be 289 last_speed = speed;
tomlankhorst 0:f3cf9865b7be 290 position_theta = fmod(1.0f*(position-position_ref+8192),163.84f);
tomlankhorst 0:f3cf9865b7be 291 position_int = floor(position_theta);
tomlankhorst 0:f3cf9865b7be 292 position_theta *= ELECTRICAL_POSITION_TO_RAD;
tomlankhorst 0:f3cf9865b7be 293 position_sin = arm_sin_f32(position_theta);
tomlankhorst 0:f3cf9865b7be 294 position_cos = arm_cos_f32(position_theta);
tomlankhorst 0:f3cf9865b7be 295
tomlankhorst 0:f3cf9865b7be 296 // Impedance controller...
JKaal 3:10863117020c 297 if(prbstest[z] == 1){
JKaal 3:10863117020c 298 torque = -0.1;//-ZControl_K*0.00076699f*(ABSPOS()-ZControl_RefPos) - ZControl_B*speed - ZControl_I*acceleration;
JKaal 3:10863117020c 299 cout << torque << ",";//"\r\n";
JKaal 3:10863117020c 300 }
JKaal 3:10863117020c 301 else if(prbstest[z] == 0){
JKaal 3:10863117020c 302 torque = 0.1;
JKaal 3:10863117020c 303 cout << torque << ",";//"\r\n";
JKaal 3:10863117020c 304 }
tomlankhorst 0:f3cf9865b7be 305
tomlankhorst 0:f3cf9865b7be 306 // Preprocess torque command
tomlankhorst 0:f3cf9865b7be 307 torque_setpoint = (torque > TORQUE_LIMIT) ? TORQUE_LIMIT : (torque < -TORQUE_LIMIT ? -TORQUE_LIMIT : torque);
tomlankhorst 0:f3cf9865b7be 308 #if ENABLE_COGGING_COMPENSATION == 1
tomlankhorst 0:f3cf9865b7be 309 torque_setpoint+= CC_GAIN*(cogging_compensation[position_int]);
tomlankhorst 0:f3cf9865b7be 310 #endif
tomlankhorst 0:f3cf9865b7be 311
tomlankhorst 0:f3cf9865b7be 312 /**
tomlankhorst 0:f3cf9865b7be 313 *F| / Stribeck + Coulomb + Viscous
tomlankhorst 0:f3cf9865b7be 314 * |\_/
tomlankhorst 0:f3cf9865b7be 315 * |____v_ */
tomlankhorst 0:f3cf9865b7be 316
tomlankhorst 0:f3cf9865b7be 317 #if ENABLE_FRICTION_COMPENSATION == 1
tomlankhorst 0:f3cf9865b7be 318 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 319 #endif
tomlankhorst 0:f3cf9865b7be 320 #if ENABLE_DITHER == 1
tomlankhorst 0:f3cf9865b7be 321 dither_tick++;
tomlankhorst 0:f3cf9865b7be 322 if(dither_tick > DITHER_TICKS) {
tomlankhorst 0:f3cf9865b7be 323 dither_tick = 0;
tomlankhorst 0:f3cf9865b7be 324 } else {
tomlankhorst 0:f3cf9865b7be 325 torque_setpoint+=DITHER_FORCE*sin(2*PI/DITHER_TICKS*dither_tick);
tomlankhorst 0:f3cf9865b7be 326 }
tomlankhorst 0:f3cf9865b7be 327 #endif
tomlankhorst 0:f3cf9865b7be 328
tomlankhorst 0:f3cf9865b7be 329 // Transform torque command
tomlankhorst 0:f3cf9865b7be 330 static float current_a_setpoint;
tomlankhorst 0:f3cf9865b7be 331 static float current_b_setpoint;
tomlankhorst 0:f3cf9865b7be 332 arm_inv_park_f32(0, torque_setpoint, &current_a_setpoint, &current_b_setpoint, position_sin, position_cos);
tomlankhorst 0:f3cf9865b7be 333
tomlankhorst 0:f3cf9865b7be 334 // PI Controller
tomlankhorst 0:f3cf9865b7be 335 static float current_a_error;
tomlankhorst 0:f3cf9865b7be 336 static float current_b_error;
tomlankhorst 0:f3cf9865b7be 337 static float current_a_int_error = 0;
tomlankhorst 0:f3cf9865b7be 338 static float current_b_int_error = 0;
tomlankhorst 0:f3cf9865b7be 339 current_a_error = current_a_setpoint - GET_CURRENT_A();
tomlankhorst 0:f3cf9865b7be 340 current_b_error = current_b_setpoint - GET_CURRENT_B();
tomlankhorst 0:f3cf9865b7be 341
tomlankhorst 0:f3cf9865b7be 342 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 343 current_a_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_a_error;
tomlankhorst 0:f3cf9865b7be 344 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 345 current_b_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_b_error;
tomlankhorst 0:f3cf9865b7be 346
tomlankhorst 0:f3cf9865b7be 347 current_a_error *= CURRENT_CONTROLLER_KP;
tomlankhorst 0:f3cf9865b7be 348 current_b_error *= CURRENT_CONTROLLER_KP;
tomlankhorst 0:f3cf9865b7be 349 current_a_error += CURRENT_CONTROLLER_KI*current_a_int_error;
tomlankhorst 0:f3cf9865b7be 350 current_b_error += CURRENT_CONTROLLER_KI*current_b_int_error;
tomlankhorst 0:f3cf9865b7be 351
tomlankhorst 0:f3cf9865b7be 352 // Apply voltages
tomlankhorst 0:f3cf9865b7be 353 driver_1a = current_a_error > 0 ? current_a_error : 0;
tomlankhorst 0:f3cf9865b7be 354 driver_2a = current_a_error < 0 ? -current_a_error : 0;
tomlankhorst 0:f3cf9865b7be 355 driver_1b = current_b_error > 0 ? current_b_error : 0;
tomlankhorst 0:f3cf9865b7be 356 driver_2b = current_b_error < 0 ? -current_b_error : 0;
tomlankhorst 0:f3cf9865b7be 357 }