Diff: main.cpp

Revision:

1:70eed554399b

Parent:

0:9753f3c2e5ca

Child:

2:b5c19d4eddcc

--- a/main.cpp	Mon Feb 16 20:16:01 2015 +0000
+++ b/main.cpp	Sat Feb 21 21:39:33 2015 +0000
@@ -3,6 +3,7 @@
 #include "shared.h"
 #include "util.h"
 #include "math.h"
+#include "isr.h"
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 
@@ -12,28 +13,15 @@
 
 DigitalOut en(_EN);
 
-DigitalIn halla(_HA);
-DigitalIn hallb(_HB);
-DigitalIn hallc(_HC);
-DigitalIn dummy(D9);
-
-/*
-InterruptIn haI(_HA);
-InterruptIn hbI(_HB);
-InterruptIn hcI(_HC);
-*/
-
-#define TAKE_A_DUMP 0
-
 AnalogIn throttle(_THROTTLE);
 AnalogIn analoga(_ANALOGA);
 AnalogIn analogb(_ANALOGB);
 
 Motor* motor;
 
-Ticker dtc_upd_ticker;
+Ticker dtc_upd_ticker, throttle_upd_ticker;
 
-int i =0;
+float throttle_read;
 
 int main() {
     en = 1;
@@ -41,106 +29,9 @@
     initTimers();
     initPins();
     initData();
-    short *buffer = (short*) malloc(10000*sizeof(short));
-    
     
     while(1) {
-        float throttle_raw = throttle;
-        throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
-        if (throttle_raw < 0.0f) throttle_raw = 0.0f;
-        motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
-        if (motor->throttle < 0.05f) {
-            motor->halt = 1;
-        } else {
-            motor->halt = 0;
-        }
-        
-        motor->analoga = analoga;
-        motor->analogb = analogb;
-        
-        
-        float ascaled = 2*(((motor->analoga-0.143)/(0.618-0.143))-0.5);
-        float bscaled = 2*(((motor->analogb-0.202)/(0.542-0.202))-0.5);
-       
-        float x = bscaled/ascaled;
-        
-        
-        unsigned int index = ((abs(x))/(M))*ATAN_TABLE_SIZE;
-        
-        if(index>2000)index=2000;
-        
-        
-        if(bscaled<0){
-            if(ascaled<0) motor->whangle = arctan[index];
-            if(ascaled>0) motor->whangle = 180 - arctan[index];
-            } 
-        
-        if(bscaled>0){
-            if(ascaled>0) motor->whangle = 180+ arctan[index];
-            if(ascaled<0) motor->whangle = 360- arctan[index];
-            }
-        
-        if(motor->whangle>360)motor->whangle=360;
-        if(motor->whangle<0)motor->whangle=0;
-        
-        if (motor->halt) continue;
-        
-        buffer[i] = motor->whangle;
-        i++;
-        if(i>=10000){
-            motor->throttle = 0.0f;
-            motor->halt = 1;
-            break;
-        }
-        
-       
-        
-        
-        /*
-        pc.printf("%f,", (motor->whangle));
-        pc.printf("\n\r");
-        */
-        
-        /*
-        if (motor->halt) continue;
-        float output = analoga;
-        buffer[i] = output;
-        if (i >= 1000) {
-         for (int j = 0; j < 1000; j++) pc.printf("%f, ", buffer[j]);
-         i = 0;
-        }    
-        i++;
-        */
-        
-        /*
-        pc.printf("%d,", index);
-        pc.printf("%f,", arctan[index]);
-        pc.printf("%f,", whangle);
-        pc.printf("ascaled positive?:");
-        pc.printf("%d,", (ascaled>0));
-        pc.printf("bscaled positive?:");
-        pc.printf("%d,", (bscaled>0));
-        pc.printf("\n\r");
-        */
-        
-        /*
-        pc.printf("amin:");
-        pc.printf("%f,", amin);
-        pc.printf("amax:");
-        pc.printf("%f,", amax);
-        pc.printf("bmin:");
-        pc.printf("%f,", bmin);
-        pc.printf("bmax:");
-        pc.printf("%f,", bmax);
-        pc.printf("a:");
-        pc.printf("%f,", ascaled);
-        pc.printf("b:");
-        pc.printf("%f,", bscaled);
-        pc.printf("\n\r");
-        */
-        
-        
-        //wait(0.1);
+        throttle_read = throttle;
+        pos_update();
     }
-    for (int i = 0; i < 10000; i++) pc.printf("%d,", buffer[i]);
 }