Bayley Wang / Mbed 2 deprecated analoghalls

potato

Dependencies:   mbed

Fork of analoghalls by N K

main.cpp@0:9753f3c2e5ca, 2015-02-16 (annotated)

Committer:
nki
Date:
Mon Feb 16 20:16:01 2015 +0000
Revision:
0:9753f3c2e5ca
Child:
1:70eed554399b
hello bayley;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nki 0:9753f3c2e5ca 1 #include "mbed.h"
nki 0:9753f3c2e5ca 2 #include "constants.h"
nki 0:9753f3c2e5ca 3 #include "shared.h"
nki 0:9753f3c2e5ca 4 #include "util.h"
nki 0:9753f3c2e5ca 5 #include "math.h"
nki 0:9753f3c2e5ca 6
nki 0:9753f3c2e5ca 7 Serial pc(SERIAL_TX, SERIAL_RX);
nki 0:9753f3c2e5ca 8
nki 0:9753f3c2e5ca 9 PwmOut pha(_PH_A);
nki 0:9753f3c2e5ca 10 PwmOut phb(_PH_B);
nki 0:9753f3c2e5ca 11 PwmOut phc(_PH_C);
nki 0:9753f3c2e5ca 12
nki 0:9753f3c2e5ca 13 DigitalOut en(_EN);
nki 0:9753f3c2e5ca 14
nki 0:9753f3c2e5ca 15 DigitalIn halla(_HA);
nki 0:9753f3c2e5ca 16 DigitalIn hallb(_HB);
nki 0:9753f3c2e5ca 17 DigitalIn hallc(_HC);
nki 0:9753f3c2e5ca 18 DigitalIn dummy(D9);
nki 0:9753f3c2e5ca 19
nki 0:9753f3c2e5ca 20 /*
nki 0:9753f3c2e5ca 21 InterruptIn haI(_HA);
nki 0:9753f3c2e5ca 22 InterruptIn hbI(_HB);
nki 0:9753f3c2e5ca 23 InterruptIn hcI(_HC);
nki 0:9753f3c2e5ca 24 */
nki 0:9753f3c2e5ca 25
nki 0:9753f3c2e5ca 26 #define TAKE_A_DUMP 0
nki 0:9753f3c2e5ca 27
nki 0:9753f3c2e5ca 28 AnalogIn throttle(_THROTTLE);
nki 0:9753f3c2e5ca 29 AnalogIn analoga(_ANALOGA);
nki 0:9753f3c2e5ca 30 AnalogIn analogb(_ANALOGB);
nki 0:9753f3c2e5ca 31
nki 0:9753f3c2e5ca 32 Motor* motor;
nki 0:9753f3c2e5ca 33
nki 0:9753f3c2e5ca 34 Ticker dtc_upd_ticker;
nki 0:9753f3c2e5ca 35
nki 0:9753f3c2e5ca 36 int i =0;
nki 0:9753f3c2e5ca 37
nki 0:9753f3c2e5ca 38 int main() {
nki 0:9753f3c2e5ca 39 en = 1;
nki 0:9753f3c2e5ca 40
nki 0:9753f3c2e5ca 41 initTimers();
nki 0:9753f3c2e5ca 42 initPins();
nki 0:9753f3c2e5ca 43 initData();
nki 0:9753f3c2e5ca 44 short *buffer = (short*) malloc(10000*sizeof(short));
nki 0:9753f3c2e5ca 45
nki 0:9753f3c2e5ca 46
nki 0:9753f3c2e5ca 47 while(1) {
nki 0:9753f3c2e5ca 48 float throttle_raw = throttle;
nki 0:9753f3c2e5ca 49 throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
nki 0:9753f3c2e5ca 50 if (throttle_raw < 0.0f) throttle_raw = 0.0f;
nki 0:9753f3c2e5ca 51 motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
nki 0:9753f3c2e5ca 52 if (motor->throttle < 0.05f) {
nki 0:9753f3c2e5ca 53 motor->halt = 1;
nki 0:9753f3c2e5ca 54 } else {
nki 0:9753f3c2e5ca 55 motor->halt = 0;
nki 0:9753f3c2e5ca 56 }
nki 0:9753f3c2e5ca 57
nki 0:9753f3c2e5ca 58 motor->analoga = analoga;
nki 0:9753f3c2e5ca 59 motor->analogb = analogb;
nki 0:9753f3c2e5ca 60
nki 0:9753f3c2e5ca 61
nki 0:9753f3c2e5ca 62 float ascaled = 2*(((motor->analoga-0.143)/(0.618-0.143))-0.5);
nki 0:9753f3c2e5ca 63 float bscaled = 2*(((motor->analogb-0.202)/(0.542-0.202))-0.5);
nki 0:9753f3c2e5ca 64
nki 0:9753f3c2e5ca 65 float x = bscaled/ascaled;
nki 0:9753f3c2e5ca 66
nki 0:9753f3c2e5ca 67
nki 0:9753f3c2e5ca 68 unsigned int index = ((abs(x))/(M))*ATAN_TABLE_SIZE;
nki 0:9753f3c2e5ca 69
nki 0:9753f3c2e5ca 70 if(index>2000)index=2000;
nki 0:9753f3c2e5ca 71
nki 0:9753f3c2e5ca 72
nki 0:9753f3c2e5ca 73 if(bscaled<0){
nki 0:9753f3c2e5ca 74 if(ascaled<0) motor->whangle = arctan[index];
nki 0:9753f3c2e5ca 75 if(ascaled>0) motor->whangle = 180 - arctan[index];
nki 0:9753f3c2e5ca 76 }
nki 0:9753f3c2e5ca 77
nki 0:9753f3c2e5ca 78 if(bscaled>0){
nki 0:9753f3c2e5ca 79 if(ascaled>0) motor->whangle = 180+ arctan[index];
nki 0:9753f3c2e5ca 80 if(ascaled<0) motor->whangle = 360- arctan[index];
nki 0:9753f3c2e5ca 81 }
nki 0:9753f3c2e5ca 82
nki 0:9753f3c2e5ca 83 if(motor->whangle>360)motor->whangle=360;
nki 0:9753f3c2e5ca 84 if(motor->whangle<0)motor->whangle=0;
nki 0:9753f3c2e5ca 85
nki 0:9753f3c2e5ca 86 if (motor->halt) continue;
nki 0:9753f3c2e5ca 87
nki 0:9753f3c2e5ca 88 buffer[i] = motor->whangle;
nki 0:9753f3c2e5ca 89 i++;
nki 0:9753f3c2e5ca 90 if(i>=10000){
nki 0:9753f3c2e5ca 91 motor->throttle = 0.0f;
nki 0:9753f3c2e5ca 92 motor->halt = 1;
nki 0:9753f3c2e5ca 93 break;
nki 0:9753f3c2e5ca 94 }
nki 0:9753f3c2e5ca 95
nki 0:9753f3c2e5ca 96
nki 0:9753f3c2e5ca 97
nki 0:9753f3c2e5ca 98
nki 0:9753f3c2e5ca 99 /*
nki 0:9753f3c2e5ca 100 pc.printf("%f,", (motor->whangle));
nki 0:9753f3c2e5ca 101 pc.printf("\n\r");
nki 0:9753f3c2e5ca 102 */
nki 0:9753f3c2e5ca 103
nki 0:9753f3c2e5ca 104 /*
nki 0:9753f3c2e5ca 105 if (motor->halt) continue;
nki 0:9753f3c2e5ca 106 float output = analoga;
nki 0:9753f3c2e5ca 107 buffer[i] = output;
nki 0:9753f3c2e5ca 108 if (i >= 1000) {
nki 0:9753f3c2e5ca 109 for (int j = 0; j < 1000; j++) pc.printf("%f, ", buffer[j]);
nki 0:9753f3c2e5ca 110 i = 0;
nki 0:9753f3c2e5ca 111 }
nki 0:9753f3c2e5ca 112 i++;
nki 0:9753f3c2e5ca 113 */
nki 0:9753f3c2e5ca 114
nki 0:9753f3c2e5ca 115 /*
nki 0:9753f3c2e5ca 116 pc.printf("%d,", index);
nki 0:9753f3c2e5ca 117 pc.printf("%f,", arctan[index]);
nki 0:9753f3c2e5ca 118 pc.printf("%f,", whangle);
nki 0:9753f3c2e5ca 119 pc.printf("ascaled positive?:");
nki 0:9753f3c2e5ca 120 pc.printf("%d,", (ascaled>0));
nki 0:9753f3c2e5ca 121 pc.printf("bscaled positive?:");
nki 0:9753f3c2e5ca 122 pc.printf("%d,", (bscaled>0));
nki 0:9753f3c2e5ca 123 pc.printf("\n\r");
nki 0:9753f3c2e5ca 124 */
nki 0:9753f3c2e5ca 125
nki 0:9753f3c2e5ca 126 /*
nki 0:9753f3c2e5ca 127 pc.printf("amin:");
nki 0:9753f3c2e5ca 128 pc.printf("%f,", amin);
nki 0:9753f3c2e5ca 129 pc.printf("amax:");
nki 0:9753f3c2e5ca 130 pc.printf("%f,", amax);
nki 0:9753f3c2e5ca 131 pc.printf("bmin:");
nki 0:9753f3c2e5ca 132 pc.printf("%f,", bmin);
nki 0:9753f3c2e5ca 133 pc.printf("bmax:");
nki 0:9753f3c2e5ca 134 pc.printf("%f,", bmax);
nki 0:9753f3c2e5ca 135 pc.printf("a:");
nki 0:9753f3c2e5ca 136 pc.printf("%f,", ascaled);
nki 0:9753f3c2e5ca 137 pc.printf("b:");
nki 0:9753f3c2e5ca 138 pc.printf("%f,", bscaled);
nki 0:9753f3c2e5ca 139 pc.printf("\n\r");
nki 0:9753f3c2e5ca 140 */
nki 0:9753f3c2e5ca 141
nki 0:9753f3c2e5ca 142
nki 0:9753f3c2e5ca 143 //wait(0.1);
nki 0:9753f3c2e5ca 144 }
nki 0:9753f3c2e5ca 145 for (int i = 0; i < 10000; i++) pc.printf("%d,", buffer[i]);
nki 0:9753f3c2e5ca 146 }