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@3:a08e9ebf1958, 2017-06-19 (annotated)

Committer:
JKaal
Date:
Mon Jun 19 07:53:15 2017 +0000
Revision:
3:a08e9ebf1958
Parent:
2:bf29d24b69dd 
Automatically moving back and forth Haptic_hid, but with a delay on the printing of the output signal of the position to the computer.

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