Diff: main.cpp

Revision:

0:9753f3c2e5ca

Child:

1:70eed554399b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 16 20:16:01 2015 +0000
@@ -0,0 +1,146 @@
+#include "mbed.h"
+#include "constants.h"
+#include "shared.h"
+#include "util.h"
+#include "math.h"
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+PwmOut pha(_PH_A);
+PwmOut phb(_PH_B);
+PwmOut phc(_PH_C);
+
+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;
+
+int i =0;
+
+int main() {
+    en = 1;
+    
+    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);
+    }
+    for (int i = 0; i < 10000; i++) pc.printf("%d,", buffer[i]);
+}