Bayley Wang / Mbed 2 deprecated analoghalls

potato

Dependencies:   mbed

Fork of analoghalls by N K

isr.cpp@1:70eed554399b, 2015-02-21 (annotated)

Committer:
bwang
Date:
Sat Feb 21 21:39:33 2015 +0000
Revision:
1:70eed554399b
Parent:
0:9753f3c2e5ca
Child:
2:b5c19d4eddcc
potato

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nki 0:9753f3c2e5ca 1 #include "isr.h"
nki 0:9753f3c2e5ca 2 #include "mbed.h"
nki 0:9753f3c2e5ca 3 #include "shared.h"
nki 0:9753f3c2e5ca 4 #include "constants.h"
nki 0:9753f3c2e5ca 5
bwang 1:70eed554399b 6 void dtc_update() {
bwang 1:70eed554399b 7 float ia = motor->angle - motor->sensor_phase;
nki 0:9753f3c2e5ca 8 if (ia < 0) ia += 360.0f;
nki 0:9753f3c2e5ca 9 if (ia >= 360.0f) ia -= 360.0f;
nki 0:9753f3c2e5ca 10 float ib = ia - 120.0f;
nki 0:9753f3c2e5ca 11 if (ib < 0) ib += 360.0f;
nki 0:9753f3c2e5ca 12 float ic = ia + 120.0f;
nki 0:9753f3c2e5ca 13 if (ic >= 360.0f) ic -= 360.0f;
nki 0:9753f3c2e5ca 14
nki 0:9753f3c2e5ca 15 motor->dtc_a = sinetab[(int) (ia)] * motor->throttle;
nki 0:9753f3c2e5ca 16 motor->dtc_b = sinetab[(int) (ib)] * motor->throttle;
nki 0:9753f3c2e5ca 17 motor->dtc_c = sinetab[(int) (ic)] * motor->throttle;
nki 0:9753f3c2e5ca 18
nki 0:9753f3c2e5ca 19 if (motor->halt) {
nki 0:9753f3c2e5ca 20 pha = 0.0f;
nki 0:9753f3c2e5ca 21 phb = 1.0f;
nki 0:9753f3c2e5ca 22 phc = 0.0f;
nki 0:9753f3c2e5ca 23 } else {
nki 0:9753f3c2e5ca 24 pha = DB_COEFF * motor->dtc_a + DB_OFFSET;
nki 0:9753f3c2e5ca 25 phb = DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET;
nki 0:9753f3c2e5ca 26 phc = DB_COEFF * motor->dtc_c + DB_OFFSET;
nki 0:9753f3c2e5ca 27 }
bwang 1:70eed554399b 28 }
bwang 1:70eed554399b 29
bwang 1:70eed554399b 30 void pos_update() {
bwang 1:70eed554399b 31 float ascaled = 2 *(((float) analoga - 0.143f)/(0.618f - 0.143f) - 0.5f);
bwang 1:70eed554399b 32 float bscaled = 2 *(((float) analogb - 0.202f)/(0.542f - 0.202f) - 0.5f);
bwang 1:70eed554399b 33
bwang 1:70eed554399b 34 float x = bscaled / ascaled;
bwang 1:70eed554399b 35
bwang 1:70eed554399b 36 unsigned int index = (abs(x) / ATAN_UPPER_BOUND)* ATAN_TABLE_SIZE;
bwang 1:70eed554399b 37
bwang 1:70eed554399b 38 if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
bwang 1:70eed554399b 39
bwang 1:70eed554399b 40 if(bscaled < 0){
bwang 1:70eed554399b 41 if(ascaled < 0) motor->angle = arctan[index];
bwang 1:70eed554399b 42 if(ascaled > 0) motor->angle = 180.0f - arctan[index];
bwang 1:70eed554399b 43 }
bwang 1:70eed554399b 44
bwang 1:70eed554399b 45 if(bscaled > 0){
bwang 1:70eed554399b 46 if(ascaled > 0) motor->angle = 180.0f + arctan[index];
bwang 1:70eed554399b 47 if(ascaled < 0) motor->angle = 360.0f - arctan[index];
bwang 1:70eed554399b 48 }
bwang 1:70eed554399b 49
bwang 1:70eed554399b 50 if(motor->angle > 360.0f) motor->angle = 360.0f;
bwang 1:70eed554399b 51 if(motor->angle < 0) motor->angle=0;
bwang 1:70eed554399b 52 }
bwang 1:70eed554399b 53
bwang 1:70eed554399b 54 void throttle_update() {
bwang 1:70eed554399b 55 float throttle_raw = throttle_read;
bwang 1:70eed554399b 56 throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
bwang 1:70eed554399b 57 if (throttle_raw < 0.0f) throttle_raw = 0.0f;
bwang 1:70eed554399b 58 motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
bwang 1:70eed554399b 59 if (motor->throttle < 0.05f) {
bwang 1:70eed554399b 60 motor->halt = 1;
bwang 1:70eed554399b 61 } else {
bwang 1:70eed554399b 62 motor->halt = 0;
bwang 1:70eed554399b 63 }
nki 0:9753f3c2e5ca 64 }