main.cpp

00001 #include "mbed.h"
00002 #include "arm_math.h"
00003 //#include "USBHID.h"
00004 #include <math.h>
00005 #include <string>
00006 #include <stdlib.h>
00007 #include "MODSERIAL.h"
00008 #include "speedestimator.h"
00009 #include "position_sensor_error.h"
00010 #include "cogging_compensation.h"
00011 #include "main.h"
00012 Ticker tickObject;
00013 
00014 #include <iostream>
00015 
00016 using namespace std;
00017 const int n = 1000;
00018 int k = 0;
00019 int z = 0;
00020 int prbstest[n];
00021 int status;
00022 int statusnew;
00023 const int p = 2*n;
00024 int positievec[p];
00025 int q = 0;
00026 
00027 /** Main function
00028  * Bootstraps the system
00029  */
00030 typedef enum z_state{/*Z_ZERO,Z_B,Z_K,Z_OFF*/Z_I,Z_NUL}z_states;
00031 
00032 void SetImpedance(float i, float b, float k, float pos)
00033 {
00034             ZControl_I = i;
00035             ZControl_B = b;
00036             ZControl_K = k;
00037             ZControl_RefPos = pos;   
00038 }
00039 
00040 void blink(void)
00041 {
00042     static z_states localstate=Z_I;
00043     switch(localstate)
00044     {
00045         /*case Z_ZERO:
00046         {
00047             localstate = Z_B; //mass
00048             SetImpedance(0,0.03,0,position);
00049             break;
00050         }
00051         case Z_B:
00052         {
00053             localstate = Z_I; //fluid
00054             SetImpedance(0.0009,0.01,0.001,position);
00055             break;
00056         }*/
00057         case Z_I:
00058         {
00059             localstate = Z_NUL;//spring
00060             SetImpedance(0.0001,0.05,0.8,0);
00061             break;
00062         }
00063         case Z_NUL:
00064         {
00065             localstate = Z_I;//spring
00066             SetImpedance(0.0001,0.05,0.8,500);
00067             break;
00068         }
00069         /*case Z_K:
00070         {
00071             localstate = Z_OFF;
00072             SetImpedance(0,0,0,position);
00073             driver_enable_a = 0;
00074             driver_enable_b = 0;
00075             break;
00076         }
00077         case Z_OFF:
00078         {
00079             localstate = Z_I;
00080             SetImpedance(0,0.00,0,position);
00081             driver_enable_a = 1;
00082             driver_enable_b = 1;
00083             break;
00084         }*/
00085         default:
00086         {
00087             localstate = Z_I;
00088             ZControl_I = 0;
00089             ZControl_B = 0;
00090             ZControl_K = 0;
00091             ZControl_RefPos = position;
00092         }
00093     }
00094     info_led_3 != info_led_3;
00095     wait_ms(300); //debounce
00096 }
00097     
00098 void printer(){
00099     int positie = GET_POSITION();
00100     positievec[q] = positie;
00101     cout << positievec[q] << ",";
00102     q = q+1;
00103     wait_ms(300);
00104     }
00105      
00106 int main()
00107 {
00108     // Initialize system
00109     //initialiseer_prbs();
00110     initialize_io();
00111     calibrate_current_sensor();
00112     calibrate_position();
00113 
00114     for (int i = 0; i <= n; i++) {
00115         prbstest[i] = rand() % 2;
00116         cout << prbstest[i] << ",";//"\r\n";
00117         }
00118               torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US);
00119         //tickObject.attach_us(&printer, 100000);  
00120     /*while(z<=n){   
00121         //torque_control();
00122         printer();
00123         //cout << z << ";\r\n";
00124         //z = z+1;
00125         wait_us(TORQUE_CONTROLLER_INTERVAL_US);
00126     }*/
00127 
00128     //send_report.length = 16;
00129     //recv_report.length = 16;
00130 
00131         cout << "\r\n";
00132         /*
00133         for(int k=0; k <= n; k++) {
00134          status = prbstest[k];
00135          int kplus = k+1;
00136          statusnew = prbstest[kplus];
00137          if (status == statusnew){
00138             //cout << "equal:\r\n";
00139             //cout << status << " " << k << "\r\n";
00140             //cout << statusnew << " " << kplus << "\r\n";
00141             }
00142          else if (status != statusnew){
00143             //cout << "unequal:\r\n";
00144             //cout << status << " " << k << "\r\n";
00145             //cout << statusnew << " " << kplus << "\r\n";
00146             blink();
00147             }
00148         }*/
00149     cout << positievec;
00150     
00151     while(1) {
00152         int32_t abspos = ABSPOS();
00153         printer();
00154         if(!user_btn)
00155             blink();
00156         //tickObject.attach(&blink, 5);*/
00157         
00158         //blink();
00159         //wait(1);  
00160         
00161         //send_report.data[3] = abspos & 0x000000ff;
00162         //send_report.data[2] = (abspos & 0x0000ff00) >> 8;
00163         //send_report.data[1] = (abspos & 0x00ff0000) >> 16;
00164         //send_report.data[0] = (abspos & 0xff000000) >> 24;
00165 
00166         //for(int i = 4; i < 16; i++){
00167         //    send_report.data[i] = 0x0;
00168         //}
00169 
00170         //Send the report
00171         //hid.send(&send_report);
00172 
00173         // Try to read
00174         //if(hid.readNB(&recv_report)) {
00175 
00176         //    ZControl_I          = (float)1e-6*((recv_report.data[3] << 24) | (recv_report.data[2] << 16) | (recv_report.data[1] << 8) | (recv_report.data[0]));
00177         //    ZControl_B          = (float)1e-6*((recv_report.data[7] << 24) | (recv_report.data[6] << 16) | (recv_report.data[5] << 8) | (recv_report.data[4]));
00178         //    ZControl_K          = (float)1e-6*((recv_report.data[11] << 24) | (recv_report.data[10] << 16) | (recv_report.data[9] << 8) | (recv_report.data[8]));
00179         //    ZControl_RefPos     = (recv_report.data[15] << 24) | (recv_report.data[14] << 16) | (recv_report.data[13] << 8) | (recv_report.data[12]);
00180         //}
00181 
00182         info_led_3 = !info_led_3;
00183         wait(0.01);
00184     }
00185     return 0;
00186 }
00187 
00188 /*//create a prbs signal
00189 void initialiseer_prbs() {
00190     int prbstest[n] = {};
00191 
00192     for (int i = 0; i <= n; i++) {
00193         prbstest[i] = rand() % 2;
00194         cout << prbstest[i];
00195     }
00196 }*/
00197 
00198 /** Sample the current sensor to determine the offset
00199  */
00200 void calibrate_current_sensor()
00201 {
00202     driver_enable_a = 0;
00203     driver_enable_b = 0;
00204     for(int i=0; i<100; i++) {
00205         current_sensor_a_offset+= 0.01f*current_sensor_a.read();
00206         current_sensor_b_offset+= 0.01f*current_sensor_b.read();
00207         wait_us(2000);
00208     }
00209     driver_enable_a = 1;
00210     driver_enable_b = 1;
00211 }
00212 
00213 /** Calibrates to find the reference position
00214  */
00215 void calibrate_position()
00216 {
00217     position_ref = 0;
00218     driver_vref_ab = 0.5;
00219     for(int i = 0; i < 10; i++) {
00220         driver_1a = 0.7;
00221         driver_2a = 0;
00222         driver_1b = 0;
00223         driver_2b = 0;
00224         wait(0.2);
00225         position_ref+= GET_POSITION();
00226         driver_1a = 0;
00227         driver_2a = 0;
00228         driver_1b = 0.7;
00229         driver_2b = 0;
00230         wait(0.01);
00231     }
00232     driver_vref_ab = 1;
00233     position_ref = position_ref/10;
00234     driver_1b = 0;
00235 }
00236 
00237 /** Initialize I/O (PWM, DigitalIn/Out, AnalogIn)
00238  */
00239 void initialize_io()
00240 {
00241     user_btn.mode(PullUp);
00242     //user_btn.rise(blink);
00243     pc.baud(9600);
00244     spi.format(14,3);
00245     driver_1a.period_us(33);
00246     driver_2a.period_us(33);
00247     driver_1b.period_us(33);
00248     driver_2b.period_us(33);
00249     driver_enable_a = 1;
00250     driver_enable_b = 1;
00251     driver_vref_ab  = 1;
00252 }
00253 
00254 /** Torque Controller function, controls the plant
00255  * This function is called on an interrupt basis by a Ticker object.
00256  * PI current controller and a Park transform for FOC
00257  */
00258 void torque_control()
00259 {
00260 
00261     // Get position
00262     static int   last_position = 0;
00263     static float last_speed = 0;
00264     static float position_sin;
00265     static float position_cos;
00266     static float position_theta;
00267     static float torque_setpoint;
00268     static int position_int;
00269     position        = GET_POSITION();
00270 #if ENABLE_POSITION_COMPENSATION == 1
00271     position       += position_sensor_error[position];
00272 #endif
00273     // Antialias
00274     if(position - last_position > POSITION_ANTIALIAS) {
00275         position_offset_count--;
00276         last_position+=8192;
00277     }
00278     if(position - last_position < -POSITION_ANTIALIAS) {
00279         position_offset_count++;
00280         last_position-=8192;
00281     }
00282 
00283     // Speed and position processing
00284     static speedEstimator speed_estimator(position);
00285     speed           = 0.00076699f*speed_estimator.get(position+POSITION_RESOLUTION*position_offset_count);  // rad/s
00286     LOWPASSIIR(acceleration, TORQUE_CONTROLLER_INTERVAL_INV*(speed - last_speed), 0.005f);
00287     last_position   = position;
00288     last_speed      = speed;
00289     position_theta  = fmod(1.0f*(position-position_ref+8192),163.84f);
00290     position_int    = floor(position_theta);
00291     position_theta *= ELECTRICAL_POSITION_TO_RAD;
00292     position_sin    = arm_sin_f32(position_theta);
00293     position_cos    = arm_cos_f32(position_theta);
00294 
00295     // Impedance controller...
00296 
00297         torque = -ZControl_K*0.00076699f*(ABSPOS()-ZControl_RefPos) - ZControl_B*speed - ZControl_I*acceleration;
00298         //cout << torque << ",";//"\r\n";
00299 
00300     // Preprocess torque command
00301     torque_setpoint = (torque > TORQUE_LIMIT) ? TORQUE_LIMIT : (torque < -TORQUE_LIMIT ? -TORQUE_LIMIT : torque);
00302 #if ENABLE_COGGING_COMPENSATION == 1
00303     torque_setpoint+= CC_GAIN*(cogging_compensation[position_int]);
00304 #endif
00305 
00306     /**
00307      *F|   /    Stribeck + Coulomb + Viscous
00308      * |\_/
00309      * |____v_ */
00310 
00311 #if ENABLE_FRICTION_COMPENSATION == 1
00312     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;
00313 #endif
00314 #if ENABLE_DITHER == 1
00315     dither_tick++;
00316     if(dither_tick > DITHER_TICKS) {
00317         dither_tick = 0;
00318     } else {
00319         torque_setpoint+=DITHER_FORCE*sin(2*PI/DITHER_TICKS*dither_tick);
00320     }
00321 #endif
00322 
00323     // Transform torque command
00324     static float current_a_setpoint;
00325     static float current_b_setpoint;
00326     arm_inv_park_f32(0, torque_setpoint, &current_a_setpoint, &current_b_setpoint, position_sin, position_cos);
00327 
00328     // PI Controller
00329     static float current_a_error;
00330     static float current_b_error;
00331     static float current_a_int_error = 0;
00332     static float current_b_int_error = 0;
00333     current_a_error = current_a_setpoint - GET_CURRENT_A();
00334     current_b_error = current_b_setpoint - GET_CURRENT_B();
00335 
00336     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))
00337         current_a_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_a_error;
00338     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))
00339         current_b_int_error += TORQUE_CONTROLLER_INTERVAL_US*1e-6*current_b_error;
00340 
00341     current_a_error *= CURRENT_CONTROLLER_KP;
00342     current_b_error *= CURRENT_CONTROLLER_KP;
00343     current_a_error += CURRENT_CONTROLLER_KI*current_a_int_error;
00344     current_b_error += CURRENT_CONTROLLER_KI*current_b_int_error;
00345 
00346     // Apply voltages
00347     driver_1a   = current_a_error > 0   ? current_a_error   : 0;
00348     driver_2a   = current_a_error < 0   ? -current_a_error  : 0;
00349     driver_1b   = current_b_error > 0   ? current_b_error   : 0;
00350     driver_2b   = current_b_error < 0   ? -current_b_error  : 0;
00351 }