main.cpp

00001 #include "mbed.h"
00002 #include "constants.h"
00003 #include "shared.h"
00004 #include "util.h"
00005 #include "math.h"
00006 
00007 Serial pc(SERIAL_TX, SERIAL_RX);
00008 
00009 PwmOut pha(_PH_A);
00010 PwmOut phb(_PH_B);
00011 PwmOut phc(_PH_C);
00012 
00013 DigitalOut en(_EN);
00014 
00015 DigitalIn halla(_HA);
00016 DigitalIn hallb(_HB);
00017 DigitalIn hallc(_HC);
00018 DigitalIn dummy(D9);
00019 
00020 /*
00021 InterruptIn haI(_HA);
00022 InterruptIn hbI(_HB);
00023 InterruptIn hcI(_HC);
00024 */
00025 
00026 #define TAKE_A_DUMP 0
00027 
00028 AnalogIn throttle(_THROTTLE);
00029 AnalogIn analoga(_ANALOGA);
00030 AnalogIn analogb(_ANALOGB);
00031 
00032 Motor* motor;
00033 
00034 Ticker dtc_upd_ticker;
00035 
00036 int i =0;
00037 
00038 int main() {
00039     en = 1;
00040     
00041     initTimers();
00042     initPins();
00043     initData();
00044     short *buffer = (short*) malloc(10000*sizeof(short));
00045     
00046     
00047     while(1) {
00048         float throttle_raw = throttle;
00049         throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
00050         if (throttle_raw < 0.0f) throttle_raw = 0.0f;
00051         motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
00052         if (motor->throttle < 0.05f) {
00053             motor->halt = 1;
00054         } else {
00055             motor->halt = 0;
00056         }
00057         
00058         motor->analoga = analoga;
00059         motor->analogb = analogb;
00060         
00061         
00062         float ascaled = 2*(((motor->analoga-0.143)/(0.618-0.143))-0.5);
00063         float bscaled = 2*(((motor->analogb-0.202)/(0.542-0.202))-0.5);
00064        
00065         float x = bscaled/ascaled;
00066         
00067         
00068         unsigned int index = ((abs(x))/(M))*ATAN_TABLE_SIZE;
00069         
00070         if(index>2000)index=2000;
00071         
00072         
00073         if(bscaled<0){
00074             if(ascaled<0) motor->whangle = arctan[index];
00075             if(ascaled>0) motor->whangle = 180 - arctan[index];
00076             } 
00077         
00078         if(bscaled>0){
00079             if(ascaled>0) motor->whangle = 180+ arctan[index];
00080             if(ascaled<0) motor->whangle = 360- arctan[index];
00081             }
00082         
00083         if(motor->whangle>360)motor->whangle=360;
00084         if(motor->whangle<0)motor->whangle=0;
00085         
00086         if (motor->halt) continue;
00087         
00088         buffer[i] = motor->whangle;
00089         i++;
00090         if(i>=10000){
00091             motor->throttle = 0.0f;
00092             motor->halt = 1;
00093             break;
00094         }
00095         
00096        
00097         
00098         
00099         /*
00100         pc.printf("%f,", (motor->whangle));
00101         pc.printf("\n\r");
00102         */
00103         
00104         /*
00105         if (motor->halt) continue;
00106         float output = analoga;
00107         buffer[i] = output;
00108         if (i >= 1000) {
00109          for (int j = 0; j < 1000; j++) pc.printf("%f, ", buffer[j]);
00110          i = 0;
00111         }    
00112         i++;
00113         */
00114         
00115         /*
00116         pc.printf("%d,", index);
00117         pc.printf("%f,", arctan[index]);
00118         pc.printf("%f,", whangle);
00119         pc.printf("ascaled positive?:");
00120         pc.printf("%d,", (ascaled>0));
00121         pc.printf("bscaled positive?:");
00122         pc.printf("%d,", (bscaled>0));
00123         pc.printf("\n\r");
00124         */
00125         
00126         /*
00127         pc.printf("amin:");
00128         pc.printf("%f,", amin);
00129         pc.printf("amax:");
00130         pc.printf("%f,", amax);
00131         pc.printf("bmin:");
00132         pc.printf("%f,", bmin);
00133         pc.printf("bmax:");
00134         pc.printf("%f,", bmax);
00135         pc.printf("a:");
00136         pc.printf("%f,", ascaled);
00137         pc.printf("b:");
00138         pc.printf("%f,", bscaled);
00139         pc.printf("\n\r");
00140         */
00141         
00142         
00143         //wait(0.1);
00144     }
00145     for (int i = 0; i < 10000; i++) pc.printf("%d,", buffer[i]);
00146 }