File content as of revision 0:9753f3c2e5ca:

#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]);
}