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@4:9d37f163d64c, 2017-06-19 (annotated)

Committer:
vsluiter
Date:
Mon Jun 19 08:24:44 2017 +0000
Revision:
4:9d37f163d64c
Parent:
3:10863117020c 
Changed from cout to printf, changed timing a bit

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