N K / Mbed 2 deprecated analoghalls_part_4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of analoghalls by Bayley Wang

isr.cpp@4:f18f6bc5e1fd, 2015-02-26 (annotated)

Committer:
bwang
Date:
Thu Feb 26 04:49:21 2015 +0000
Revision:
4:f18f6bc5e1fd
Parent:
3:86ccde39f61b
Child:
5:eeb8af99cb6c
latest;

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
bwang 3:86ccde39f61b 66 if (motor->halt || motor->debug_stop) {
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 4:f18f6bc5e1fd 78 float ascaled = 2*(((float)analoga-0.256f)/(0.484f-0.256f)-0.5f);
bwang 4:f18f6bc5e1fd 79 float bscaled = 2*(((float)analogb-0.254f)/(0.474f-0.254f)-0.5f);
bwang 4:f18f6bc5e1fd 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 4:f18f6bc5e1fd 86
bwang 4:f18f6bc5e1fd 87 if(ascaled<0){
bwang 4:f18f6bc5e1fd 88 if(bscaled<0) motor->angle = 90 - arctan[index];
bwang 4:f18f6bc5e1fd 89 if(bscaled>0) motor->angle = 90 + arctan[index];
bwang 4:f18f6bc5e1fd 90 }
bwang 4:f18f6bc5e1fd 91
bwang 4:f18f6bc5e1fd 92 if(ascaled>0){
bwang 4:f18f6bc5e1fd 93 if(bscaled>0) motor->angle = 270 - arctan[index];
bwang 4:f18f6bc5e1fd 94 if(bscaled<0) motor->angle = 270 + arctan[index];
bwang 4:f18f6bc5e1fd 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 3:86ccde39f61b 99
bwang 3:86ccde39f61b 100 #ifdef __DEBUG
bwang 4:f18f6bc5e1fd 101 skipidx++;
bwang 4:f18f6bc5e1fd 102 if (skipidx == SKIP) {
bwang 4:f18f6bc5e1fd 103 skipidx = 0;
bwang 4:f18f6bc5e1fd 104 if (!motor->halt) {
bwang 4:f18f6bc5e1fd 105 fbuffer[bufidx] = motor->angle;
bwang 4:f18f6bc5e1fd 106 bufidx++;
bwang 4:f18f6bc5e1fd 107 }
bwang 3:86ccde39f61b 108 }
bwang 4:f18f6bc5e1fd 109 if (bufidx == DBG_BUF_SZ) {
bwang 3:86ccde39f61b 110 motor->debug_stop = 1;
bwang 3:86ccde39f61b 111 }
bwang 3:86ccde39f61b 112 #endif
bwang 1:70eed554399b 113 }
bwang 1:70eed554399b 114
bwang 1:70eed554399b 115 void throttle_update() {
bwang 1:70eed554399b 116 float throttle_raw = throttle_read;
bwang 1:70eed554399b 117 throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
bwang 1:70eed554399b 118 if (throttle_raw < 0.0f) throttle_raw = 0.0f;
bwang 1:70eed554399b 119 motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
bwang 1:70eed554399b 120 if (motor->throttle < 0.05f) {
bwang 1:70eed554399b 121 motor->halt = 1;
bwang 1:70eed554399b 122 } else {
bwang 1:70eed554399b 123 motor->halt = 0;
bwang 1:70eed554399b 124 }
nki 0:9753f3c2e5ca 125 }