N K / Mbed 2 deprecated analoghalls_part_4

yup

Dependencies:   mbed

Fork of analoghalls by Bayley Wang

isr.cpp@2:b5c19d4eddcc, 2015-02-23 (annotated)

Committer:
bwang
Date:
Mon Feb 23 19:42:50 2015 +0000
Revision:
2:b5c19d4eddcc
Parent:
1:70eed554399b
Child:
3:86ccde39f61b
stuff

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"
bwang 2:b5c19d4eddcc 5 #include "util.h"
nki 0:9753f3c2e5ca 6
bwang 1:70eed554399b 7 void dtc_update() {
bwang 2:b5c19d4eddcc 8 #ifndef __SVM
bwang 1:70eed554399b 9 float ia = motor->angle - motor->sensor_phase;
nki 0:9753f3c2e5ca 10 if (ia < 0) ia += 360.0f;
nki 0:9753f3c2e5ca 11 if (ia >= 360.0f) ia -= 360.0f;
nki 0:9753f3c2e5ca 12 float ib = ia - 120.0f;
nki 0:9753f3c2e5ca 13 if (ib < 0) ib += 360.0f;
nki 0:9753f3c2e5ca 14 float ic = ia + 120.0f;
nki 0:9753f3c2e5ca 15 if (ic >= 360.0f) ic -= 360.0f;
nki 0:9753f3c2e5ca 16
nki 0:9753f3c2e5ca 17 motor->dtc_a = sinetab[(int) (ia)] * motor->throttle;
nki 0:9753f3c2e5ca 18 motor->dtc_b = sinetab[(int) (ib)] * motor->throttle;
nki 0:9753f3c2e5ca 19 motor->dtc_c = sinetab[(int) (ic)] * motor->throttle;
bwang 2:b5c19d4eddcc 20 #else
bwang 2:b5c19d4eddcc 21 float ix = 120.0f - motor->angle;
bwang 2:b5c19d4eddcc 22 if (ix < 0) ix += 360.0f;
bwang 2:b5c19d4eddcc 23 if (ix >= 360.0f) ix -= 360.0f;
bwang 2:b5c19d4eddcc 24 float dx = sinetab[(int) ix];
bwang 2:b5c19d4eddcc 25 float iy = motor->angle;
bwang 2:b5c19d4eddcc 26 if (iy < 0) iy += 360.0f;
bwang 2:b5c19d4eddcc 27 if (iy >= 360.0f) iy -= 360.0f;
bwang 2:b5c19d4eddcc 28 float dy = sinetab[(int) iy];
bwang 2:b5c19d4eddcc 29 int sector = (int) (iy / 60.0f) + 1;
nki 0:9753f3c2e5ca 30
bwang 2:b5c19d4eddcc 31 switch(sector) {
bwang 2:b5c19d4eddcc 32 case 1:
bwang 2:b5c19d4eddcc 33 motor->dtc_a = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 34 motor->dtc_b = 1.0f + dx - dy;
bwang 2:b5c19d4eddcc 35 motor->dtc_c = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 36 break;
bwang 2:b5c19d4eddcc 37 case 2:
bwang 2:b5c19d4eddcc 38 motor->dtc_a = 1.0f - dx + dy;
bwang 2:b5c19d4eddcc 39 motor->dtc_b = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 40 motor->dtc_c = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 41 break;
bwang 2:b5c19d4eddcc 42 case 3:
bwang 2:b5c19d4eddcc 43 motor->dtc_a = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 44 motor->dtc_b = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 45 motor->dtc_c = 1.0f + dx - dy;
bwang 2:b5c19d4eddcc 46 break;
bwang 2:b5c19d4eddcc 47 case 4:
bwang 2:b5c19d4eddcc 48 motor->dtc_a = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 49 motor->dtc_b = 1.0f - dx + dy;
bwang 2:b5c19d4eddcc 50 motor->dtc_c = 1.0f - dx + dy; //suspect
bwang 2:b5c19d4eddcc 51 break;
bwang 2:b5c19d4eddcc 52 case 5:
bwang 2:b5c19d4eddcc 53 motor->dtc_a = 1.0f + dx - dy;
bwang 2:b5c19d4eddcc 54 motor->dtc_b = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 55 motor->dtc_c = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 56 break;
bwang 2:b5c19d4eddcc 57 case 6:
bwang 2:b5c19d4eddcc 58 motor->dtc_a = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 59 motor->dtc_b = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 60 motor->dtc_c = 1.0f - dx + dy;
bwang 2:b5c19d4eddcc 61 break;
bwang 2:b5c19d4eddcc 62 default:
bwang 2:b5c19d4eddcc 63 break;
bwang 2:b5c19d4eddcc 64 }
bwang 2:b5c19d4eddcc 65 #endif
nki 0:9753f3c2e5ca 66 if (motor->halt) {
bwang 2:b5c19d4eddcc 67 setDtcA(0);
bwang 2:b5c19d4eddcc 68 setDtcB(1.0f);
bwang 2:b5c19d4eddcc 69 setDtcC(0);
nki 0:9753f3c2e5ca 70 } else {
bwang 2:b5c19d4eddcc 71 setDtcA(DB_COEFF * motor->dtc_a + DB_OFFSET);
bwang 2:b5c19d4eddcc 72 setDtcB(DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET);
bwang 2:b5c19d4eddcc 73 setDtcC(DB_COEFF * motor->dtc_c + DB_OFFSET);
nki 0:9753f3c2e5ca 74 }
bwang 1:70eed554399b 75 }
bwang 1:70eed554399b 76
bwang 1:70eed554399b 77 void pos_update() {
bwang 1:70eed554399b 78 float ascaled = 2 *(((float) analoga - 0.143f)/(0.618f - 0.143f) - 0.5f);
bwang 1:70eed554399b 79 float bscaled = 2 *(((float) analogb - 0.202f)/(0.542f - 0.202f) - 0.5f);
bwang 1:70eed554399b 80
bwang 1:70eed554399b 81 float x = bscaled / ascaled;
bwang 1:70eed554399b 82
bwang 1:70eed554399b 83 unsigned int index = (abs(x) / ATAN_UPPER_BOUND)* ATAN_TABLE_SIZE;
bwang 1:70eed554399b 84
bwang 1:70eed554399b 85 if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
bwang 1:70eed554399b 86
bwang 1:70eed554399b 87 if(bscaled < 0){
bwang 1:70eed554399b 88 if(ascaled < 0) motor->angle = arctan[index];
bwang 1:70eed554399b 89 if(ascaled > 0) motor->angle = 180.0f - arctan[index];
bwang 1:70eed554399b 90 }
bwang 1:70eed554399b 91
bwang 1:70eed554399b 92 if(bscaled > 0){
bwang 1:70eed554399b 93 if(ascaled > 0) motor->angle = 180.0f + arctan[index];
bwang 1:70eed554399b 94 if(ascaled < 0) motor->angle = 360.0f - arctan[index];
bwang 1:70eed554399b 95 }
bwang 1:70eed554399b 96
bwang 1:70eed554399b 97 if(motor->angle > 360.0f) motor->angle = 360.0f;
bwang 2:b5c19d4eddcc 98 if(motor->angle < 0) motor->angle = 0;
bwang 1:70eed554399b 99 }
bwang 1:70eed554399b 100
bwang 1:70eed554399b 101 void throttle_update() {
bwang 1:70eed554399b 102 float throttle_raw = throttle_read;
bwang 1:70eed554399b 103 throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
bwang 1:70eed554399b 104 if (throttle_raw < 0.0f) throttle_raw = 0.0f;
bwang 1:70eed554399b 105 motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
bwang 1:70eed554399b 106 if (motor->throttle < 0.05f) {
bwang 1:70eed554399b 107 motor->halt = 1;
bwang 1:70eed554399b 108 } else {
bwang 1:70eed554399b 109 motor->halt = 0;
bwang 1:70eed554399b 110 }
nki 0:9753f3c2e5ca 111 }