N K / Mbed 2 deprecated analoghalls_part_4

yup

Dependencies:   mbed

Fork of analoghalls by Bayley Wang

isr.cpp@6:4960629abb90, 2015-02-26 (annotated)

Committer:
nki
Date:
Thu Feb 26 14:09:19 2015 +0000
Revision:
6:4960629abb90
Parent:
5:eeb8af99cb6c 
LOLOL PROPROTIONAL CURENT CONTROL;

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 5:eeb8af99cb6c 6 #include "math.h"
nki 0:9753f3c2e5ca 7
bwang 1:70eed554399b 8 void dtc_update() {
bwang 2:b5c19d4eddcc 9 #ifndef __SVM
bwang 1:70eed554399b 10 float ia = motor->angle - motor->sensor_phase;
nki 0:9753f3c2e5ca 11 if (ia < 0) ia += 360.0f;
nki 0:9753f3c2e5ca 12 if (ia >= 360.0f) ia -= 360.0f;
nki 0:9753f3c2e5ca 13 float ib = ia - 120.0f;
nki 0:9753f3c2e5ca 14 if (ib < 0) ib += 360.0f;
nki 0:9753f3c2e5ca 15 float ic = ia + 120.0f;
nki 0:9753f3c2e5ca 16 if (ic >= 360.0f) ic -= 360.0f;
nki 0:9753f3c2e5ca 17
nki 6:4960629abb90 18 motor->dtc_a = sinetab[(int) (ia)] * motor->dtc;
nki 6:4960629abb90 19 motor->dtc_b = sinetab[(int) (ib)] * motor->dtc;
nki 6:4960629abb90 20 motor->dtc_c = sinetab[(int) (ic)] * motor->dtc;
bwang 2:b5c19d4eddcc 21 #else
bwang 2:b5c19d4eddcc 22 float ix = 120.0f - motor->angle;
bwang 2:b5c19d4eddcc 23 if (ix < 0) ix += 360.0f;
bwang 2:b5c19d4eddcc 24 if (ix >= 360.0f) ix -= 360.0f;
bwang 2:b5c19d4eddcc 25 float dx = sinetab[(int) ix];
bwang 2:b5c19d4eddcc 26 float iy = motor->angle;
bwang 2:b5c19d4eddcc 27 if (iy < 0) iy += 360.0f;
bwang 2:b5c19d4eddcc 28 if (iy >= 360.0f) iy -= 360.0f;
bwang 2:b5c19d4eddcc 29 float dy = sinetab[(int) iy];
bwang 2:b5c19d4eddcc 30 int sector = (int) (iy / 60.0f) + 1;
nki 0:9753f3c2e5ca 31
bwang 2:b5c19d4eddcc 32 switch(sector) {
bwang 2:b5c19d4eddcc 33 case 1:
bwang 2:b5c19d4eddcc 34 motor->dtc_a = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 35 motor->dtc_b = 1.0f + dx - dy;
bwang 2:b5c19d4eddcc 36 motor->dtc_c = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 37 break;
bwang 2:b5c19d4eddcc 38 case 2:
bwang 2:b5c19d4eddcc 39 motor->dtc_a = 1.0f - dx + dy;
bwang 2:b5c19d4eddcc 40 motor->dtc_b = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 41 motor->dtc_c = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 42 break;
bwang 2:b5c19d4eddcc 43 case 3:
bwang 2:b5c19d4eddcc 44 motor->dtc_a = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 45 motor->dtc_b = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 46 motor->dtc_c = 1.0f + dx - dy;
bwang 2:b5c19d4eddcc 47 break;
bwang 2:b5c19d4eddcc 48 case 4:
bwang 2:b5c19d4eddcc 49 motor->dtc_a = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 50 motor->dtc_b = 1.0f - dx + dy;
bwang 2:b5c19d4eddcc 51 motor->dtc_c = 1.0f - dx + dy; //suspect
bwang 2:b5c19d4eddcc 52 break;
bwang 2:b5c19d4eddcc 53 case 5:
bwang 2:b5c19d4eddcc 54 motor->dtc_a = 1.0f + dx - dy;
bwang 2:b5c19d4eddcc 55 motor->dtc_b = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 56 motor->dtc_c = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 57 break;
bwang 2:b5c19d4eddcc 58 case 6:
bwang 2:b5c19d4eddcc 59 motor->dtc_a = 1.0f - dx - dy;
bwang 2:b5c19d4eddcc 60 motor->dtc_b = 1.0f + dx + dy;
bwang 2:b5c19d4eddcc 61 motor->dtc_c = 1.0f - dx + dy;
bwang 2:b5c19d4eddcc 62 break;
bwang 2:b5c19d4eddcc 63 default:
bwang 2:b5c19d4eddcc 64 break;
bwang 2:b5c19d4eddcc 65 }
bwang 2:b5c19d4eddcc 66 #endif
bwang 3:86ccde39f61b 67 if (motor->halt || motor->debug_stop) {
bwang 2:b5c19d4eddcc 68 setDtcA(0);
bwang 2:b5c19d4eddcc 69 setDtcB(1.0f);
bwang 2:b5c19d4eddcc 70 setDtcC(0);
nki 0:9753f3c2e5ca 71 } else {
bwang 2:b5c19d4eddcc 72 setDtcA(DB_COEFF * motor->dtc_a + DB_OFFSET);
bwang 2:b5c19d4eddcc 73 setDtcB(DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET);
bwang 2:b5c19d4eddcc 74 setDtcC(DB_COEFF * motor->dtc_c + DB_OFFSET);
nki 0:9753f3c2e5ca 75 }
bwang 1:70eed554399b 76 }
bwang 1:70eed554399b 77
bwang 1:70eed554399b 78 void pos_update() {
bwang 4:f18f6bc5e1fd 79 float ascaled = 2*(((float)analoga-0.256f)/(0.484f-0.256f)-0.5f);
bwang 4:f18f6bc5e1fd 80 float bscaled = 2*(((float)analogb-0.254f)/(0.474f-0.254f)-0.5f);
bwang 4:f18f6bc5e1fd 81
bwang 1:70eed554399b 82 float x = bscaled / ascaled;
bwang 1:70eed554399b 83
bwang 1:70eed554399b 84 unsigned int index = (abs(x) / ATAN_UPPER_BOUND)* ATAN_TABLE_SIZE;
bwang 1:70eed554399b 85
bwang 1:70eed554399b 86 if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
bwang 4:f18f6bc5e1fd 87
bwang 4:f18f6bc5e1fd 88 if(ascaled<0){
bwang 4:f18f6bc5e1fd 89 if(bscaled<0) motor->angle = 90 - arctan[index];
bwang 4:f18f6bc5e1fd 90 if(bscaled>0) motor->angle = 90 + arctan[index];
bwang 4:f18f6bc5e1fd 91 }
bwang 4:f18f6bc5e1fd 92
bwang 4:f18f6bc5e1fd 93 if(ascaled>0){
bwang 4:f18f6bc5e1fd 94 if(bscaled>0) motor->angle = 270 - arctan[index];
bwang 4:f18f6bc5e1fd 95 if(bscaled<0) motor->angle = 270 + arctan[index];
bwang 4:f18f6bc5e1fd 96 }
bwang 1:70eed554399b 97
bwang 1:70eed554399b 98 if(motor->angle > 360.0f) motor->angle = 360.0f;
bwang 2:b5c19d4eddcc 99 if(motor->angle < 0) motor->angle = 0;
bwang 3:86ccde39f61b 100
nki 5:eeb8af99cb6c 101 //calculate rotational rate
nki 5:eeb8af99cb6c 102
nki 5:eeb8af99cb6c 103 float speed_raw = motor->angle - motor->lastangle;
nki 5:eeb8af99cb6c 104 //these IFs check for theta = 0 crossings
nki 5:eeb8af99cb6c 105 if(speed_raw < -180.0f){
nki 5:eeb8af99cb6c 106 speed_raw = motor->angle - motor->lastangle + 360.0f;
nki 5:eeb8af99cb6c 107 }
nki 5:eeb8af99cb6c 108 if(speed_raw > 180.0f){
nki 5:eeb8af99cb6c 109 speed_raw = motor->angle - motor->lastangle - 360.0f;
nki 5:eeb8af99cb6c 110 }
nki 5:eeb8af99cb6c 111 motor->lastangle = motor->angle;
nki 5:eeb8af99cb6c 112
nki 5:eeb8af99cb6c 113 motor->speed = (1.0f - SPEED_LPF) * speed_raw + SPEED_LPF * motor->speed;
nki 5:eeb8af99cb6c 114
bwang 3:86ccde39f61b 115 #ifdef __DEBUG
bwang 4:f18f6bc5e1fd 116 skipidx++;
bwang 4:f18f6bc5e1fd 117 if (skipidx == SKIP) {
bwang 4:f18f6bc5e1fd 118 skipidx = 0;
bwang 4:f18f6bc5e1fd 119 if (!motor->halt) {
bwang 4:f18f6bc5e1fd 120 fbuffer[bufidx] = motor->angle;
bwang 4:f18f6bc5e1fd 121 bufidx++;
bwang 4:f18f6bc5e1fd 122 }
bwang 3:86ccde39f61b 123 }
bwang 4:f18f6bc5e1fd 124 if (bufidx == DBG_BUF_SZ) {
bwang 3:86ccde39f61b 125 motor->debug_stop = 1;
bwang 3:86ccde39f61b 126 }
bwang 3:86ccde39f61b 127 #endif
bwang 1:70eed554399b 128 }
bwang 1:70eed554399b 129
bwang 1:70eed554399b 130 void throttle_update() {
bwang 1:70eed554399b 131 float throttle_raw = throttle_read;
bwang 1:70eed554399b 132 throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
bwang 1:70eed554399b 133 if (throttle_raw < 0.0f) throttle_raw = 0.0f;
bwang 1:70eed554399b 134 motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
nki 6:4960629abb90 135 motor->command = motor->throttle;
nki 5:eeb8af99cb6c 136 if (motor->throttle < 0.05f & abs(motor->speed) < 1.0f) {
bwang 1:70eed554399b 137 motor->halt = 1;
bwang 1:70eed554399b 138 } else {
bwang 1:70eed554399b 139 motor->halt = 0;
bwang 1:70eed554399b 140 }
nki 5:eeb8af99cb6c 141 }
nki 5:eeb8af99cb6c 142
nki 5:eeb8af99cb6c 143 void isense_update() {
nki 6:4960629abb90 144
nki 5:eeb8af99cb6c 145 float ia = ia_read;
nki 5:eeb8af99cb6c 146 float ib = ib_read;
nki 6:4960629abb90 147
nki 6:4960629abb90 148 if(motor->halt == 1){ //zero the current sensors
nki 6:4960629abb90 149 motor->iazero = ia;
nki 6:4960629abb90 150 motor->ibzero = ib;
nki 6:4960629abb90 151 }
nki 6:4960629abb90 152
nki 6:4960629abb90 153 ia = ia-motor->iazero;
nki 6:4960629abb90 154 ib = ib-motor->ibzero;
nki 6:4960629abb90 155
nki 6:4960629abb90 156 float irms = ia*ia + ib*ib + (ia+ib)*(ia+ib);
nki 6:4960629abb90 157
nki 6:4960629abb90 158 motor->current = (1.0f - CURRENT_LPF) * irms + CURRENT_LPF * motor->current;
nki 6:4960629abb90 159
nki 6:4960629abb90 160 float iset = IMAX * motor->command;
nki 6:4960629abb90 161 motor->iP = iset - motor->current;
nki 6:4960629abb90 162 // motor->iI = motor->iIlast + motor->iP;
nki 6:4960629abb90 163 // motor->iD = motor->iP - motor->ilast;
nki 6:4960629abb90 164 motor->dtc = Kp_i*motor->iP; //+ Ki_i*motor->iI + Kd_i*motor->iD;
nki 6:4960629abb90 165
nki 6:4960629abb90 166 if(motor->dtc < 0.0f){
nki 6:4960629abb90 167 motor->dtc = 0.0f;
nki 6:4960629abb90 168 }
nki 6:4960629abb90 169 if(motor->dtc > 1.0f){
nki 6:4960629abb90 170 motor->dtc = 1.0f;
nki 6:4960629abb90 171 }
nki 6:4960629abb90 172
nki 5:eeb8af99cb6c 173
nki 5:eeb8af99cb6c 174
nki 5:eeb8af99cb6c 175
nki 5:eeb8af99cb6c 176 //float error = irms - iset
nki 0:9753f3c2e5ca 177 }