Kiko Ishimoto
/
robocon2017mbed_nedoControl_L
nedo用 未検証
main.cpp@0:dc3a10b60a56, 2017-11-10 (annotated)
- Committer:
- kikoaac
- Date:
- Fri Nov 10 14:33:52 2017 +0000
- Revision:
- 0:dc3a10b60a56
nedo?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kikoaac | 0:dc3a10b60a56 | 1 | #include "mbed.h" |
kikoaac | 0:dc3a10b60a56 | 2 | #define R 0 |
kikoaac | 0:dc3a10b60a56 | 3 | #define L 1 |
kikoaac | 0:dc3a10b60a56 | 4 | |
kikoaac | 0:dc3a10b60a56 | 5 | #define DEBUG 0 |
kikoaac | 0:dc3a10b60a56 | 6 | #define DEBUG_R 1 |
kikoaac | 0:dc3a10b60a56 | 7 | #define CALIB 0 |
kikoaac | 0:dc3a10b60a56 | 8 | |
kikoaac | 0:dc3a10b60a56 | 9 | #define SetupCMD 'A' |
kikoaac | 0:dc3a10b60a56 | 10 | #define Respons 'S' |
kikoaac | 0:dc3a10b60a56 | 11 | DigitalOut debugLed1(D6); |
kikoaac | 0:dc3a10b60a56 | 12 | DigitalOut debugLed2(D7); |
kikoaac | 0:dc3a10b60a56 | 13 | bool send = false; |
kikoaac | 0:dc3a10b60a56 | 14 | union floatInByte |
kikoaac | 0:dc3a10b60a56 | 15 | { |
kikoaac | 0:dc3a10b60a56 | 16 | uint16_t si; |
kikoaac | 0:dc3a10b60a56 | 17 | unsigned char c[2]; |
kikoaac | 0:dc3a10b60a56 | 18 | }; |
kikoaac | 0:dc3a10b60a56 | 19 | class AnalogInLPF : public AnalogIn |
kikoaac | 0:dc3a10b60a56 | 20 | { |
kikoaac | 0:dc3a10b60a56 | 21 | private: |
kikoaac | 0:dc3a10b60a56 | 22 | float alpha; |
kikoaac | 0:dc3a10b60a56 | 23 | float prevAnalog; |
kikoaac | 0:dc3a10b60a56 | 24 | float nowAnalog; |
kikoaac | 0:dc3a10b60a56 | 25 | public : AnalogInLPF(PinName pin,float alpha_) : AnalogIn(pin) |
kikoaac | 0:dc3a10b60a56 | 26 | { |
kikoaac | 0:dc3a10b60a56 | 27 | alpha = alpha_; |
kikoaac | 0:dc3a10b60a56 | 28 | prevAnalog = 0.0; |
kikoaac | 0:dc3a10b60a56 | 29 | } |
kikoaac | 0:dc3a10b60a56 | 30 | float read(){ |
kikoaac | 0:dc3a10b60a56 | 31 | nowAnalog = AnalogIn::read(); |
kikoaac | 0:dc3a10b60a56 | 32 | nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; |
kikoaac | 0:dc3a10b60a56 | 33 | prevAnalog = nowAnalog; |
kikoaac | 0:dc3a10b60a56 | 34 | return nowAnalog; |
kikoaac | 0:dc3a10b60a56 | 35 | } |
kikoaac | 0:dc3a10b60a56 | 36 | short read_u16(){ |
kikoaac | 0:dc3a10b60a56 | 37 | nowAnalog = AnalogIn::read(); |
kikoaac | 0:dc3a10b60a56 | 38 | nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; |
kikoaac | 0:dc3a10b60a56 | 39 | nowAnalog = float(short(nowAnalog*0xFFFF)&0xFF00)/0xFFFF; |
kikoaac | 0:dc3a10b60a56 | 40 | prevAnalog = nowAnalog; |
kikoaac | 0:dc3a10b60a56 | 41 | return short(nowAnalog*0xFFFF); |
kikoaac | 0:dc3a10b60a56 | 42 | } |
kikoaac | 0:dc3a10b60a56 | 43 | }; |
kikoaac | 0:dc3a10b60a56 | 44 | class InLPF |
kikoaac | 0:dc3a10b60a56 | 45 | { |
kikoaac | 0:dc3a10b60a56 | 46 | private: |
kikoaac | 0:dc3a10b60a56 | 47 | float alpha; |
kikoaac | 0:dc3a10b60a56 | 48 | float prevAnalog; |
kikoaac | 0:dc3a10b60a56 | 49 | float nowAnalog; |
kikoaac | 0:dc3a10b60a56 | 50 | public : InLPF(float alpha_ = 0.2) |
kikoaac | 0:dc3a10b60a56 | 51 | { |
kikoaac | 0:dc3a10b60a56 | 52 | alpha = alpha_; |
kikoaac | 0:dc3a10b60a56 | 53 | prevAnalog = 0.0; |
kikoaac | 0:dc3a10b60a56 | 54 | } |
kikoaac | 0:dc3a10b60a56 | 55 | float read(float in){ |
kikoaac | 0:dc3a10b60a56 | 56 | nowAnalog = in; |
kikoaac | 0:dc3a10b60a56 | 57 | nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; |
kikoaac | 0:dc3a10b60a56 | 58 | prevAnalog = nowAnalog; |
kikoaac | 0:dc3a10b60a56 | 59 | return nowAnalog; |
kikoaac | 0:dc3a10b60a56 | 60 | } |
kikoaac | 0:dc3a10b60a56 | 61 | short read_u16(float in){ |
kikoaac | 0:dc3a10b60a56 | 62 | nowAnalog = in; |
kikoaac | 0:dc3a10b60a56 | 63 | nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; |
kikoaac | 0:dc3a10b60a56 | 64 | prevAnalog = nowAnalog; |
kikoaac | 0:dc3a10b60a56 | 65 | return short(nowAnalog*0xFFFF)&0xFF00; |
kikoaac | 0:dc3a10b60a56 | 66 | } |
kikoaac | 0:dc3a10b60a56 | 67 | }; |
kikoaac | 0:dc3a10b60a56 | 68 | uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax); |
kikoaac | 0:dc3a10b60a56 | 69 | #define LP 0.30 |
kikoaac | 0:dc3a10b60a56 | 70 | AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP-0.018),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP),AnalogInLPF(A3,LP)}; |
kikoaac | 0:dc3a10b60a56 | 71 | InLPF ArmSense2[4] = {InLPF(LP),InLPF(LP),InLPF(LP),InLPF(LP)}; |
kikoaac | 0:dc3a10b60a56 | 72 | #if !CALIB |
kikoaac | 0:dc3a10b60a56 | 73 | //1号機 |
kikoaac | 0:dc3a10b60a56 | 74 | /* |
kikoaac | 0:dc3a10b60a56 | 75 | uint16_t MinimumRangeR[4] = {12800,41414,26368,11776}; |
kikoaac | 0:dc3a10b60a56 | 76 | uint16_t MaxmumRangeR[4] = {36608,52242,55552,38144}; |
kikoaac | 0:dc3a10b60a56 | 77 | uint16_t MinimumRangeL[4] = {23040,24832,24576,32256}; |
kikoaac | 0:dc3a10b60a56 | 78 | uint16_t MaxmumRangeL[4] = {44312,47296,59392,58624}; |
kikoaac | 0:dc3a10b60a56 | 79 | |
kikoaac | 0:dc3a10b60a56 | 80 | bool ReverseL[4] = {false,true,true,false}; |
kikoaac | 0:dc3a10b60a56 | 81 | bool ReverseR[4] = {true,false,true,false}; |
kikoaac | 0:dc3a10b60a56 | 82 | */ |
kikoaac | 0:dc3a10b60a56 | 83 | /* |
kikoaac | 0:dc3a10b60a56 | 84 | uint16_t MinimumRangeR[4] = {14592,41414,16128,12288}; |
kikoaac | 0:dc3a10b60a56 | 85 | uint16_t MaxmumRangeR[4] = {41216,52242,61952,38144}; |
kikoaac | 0:dc3a10b60a56 | 86 | uint16_t MinimumRangeL[4] = {23040,18176,11008,34303}; |
kikoaac | 0:dc3a10b60a56 | 87 | uint16_t MaxmumRangeL[4] = {44312,47296,60106,58368}; |
kikoaac | 0:dc3a10b60a56 | 88 | |
kikoaac | 0:dc3a10b60a56 | 89 | bool ReverseL[4] = {false,true,true,false}; |
kikoaac | 0:dc3a10b60a56 | 90 | bool ReverseR[4] = {true,false,true,false}; |
kikoaac | 0:dc3a10b60a56 | 91 | */ |
kikoaac | 0:dc3a10b60a56 | 92 | /* |
kikoaac | 0:dc3a10b60a56 | 93 | uint16_t MinimumRangeR[4] = {1280,42240,36096,17152}; |
kikoaac | 0:dc3a10b60a56 | 94 | uint16_t MaxmumRangeR[4] = {45312,52992,58624,41216}; |
kikoaac | 0:dc3a10b60a56 | 95 | uint16_t MinimumRangeL[4] = {23040,18432,24576,58112}; |
kikoaac | 0:dc3a10b60a56 | 96 | uint16_t MaxmumRangeL[4] = {45312,58112,59392,30976}; |
kikoaac | 0:dc3a10b60a56 | 97 | */ |
kikoaac | 0:dc3a10b60a56 | 98 | //2号機 |
kikoaac | 0:dc3a10b60a56 | 99 | //17468 23308 |
kikoaac | 0:dc3a10b60a56 | 100 | |
kikoaac | 0:dc3a10b60a56 | 101 | uint16_t MinimumRangeR[4] = {19900,41216,45056,13002}; |
kikoaac | 0:dc3a10b60a56 | 102 | uint16_t MaxmumRangeR[4] = {42544,53248,60676,38312}; |
kikoaac | 0:dc3a10b60a56 | 103 | uint16_t MinimumRangeL[4] = {21500,19432,10190,55588}; |
kikoaac | 0:dc3a10b60a56 | 104 | uint16_t MaxmumRangeL[4] = {43520,43300,43264,30208}; |
kikoaac | 0:dc3a10b60a56 | 105 | |
kikoaac | 0:dc3a10b60a56 | 106 | bool ReverseL[4] = {false,true,true,true}; |
kikoaac | 0:dc3a10b60a56 | 107 | bool ReverseR[4] = {true,false,true,false}; |
kikoaac | 0:dc3a10b60a56 | 108 | |
kikoaac | 0:dc3a10b60a56 | 109 | #endif |
kikoaac | 0:dc3a10b60a56 | 110 | #if CALIB |
kikoaac | 0:dc3a10b60a56 | 111 | uint16_t MinimumRangeR[4] = {0,0,0,0}; |
kikoaac | 0:dc3a10b60a56 | 112 | uint16_t MaxmumRangeR[4] = {0xffff,0xffff,0xffff,0xffff}; |
kikoaac | 0:dc3a10b60a56 | 113 | uint16_t MinimumRangeL[4] = {0,0,0,0}; |
kikoaac | 0:dc3a10b60a56 | 114 | uint16_t MaxmumRangeL[4] = {0xffff,0xffff,0xffff,0xffff}; |
kikoaac | 0:dc3a10b60a56 | 115 | #endif |
kikoaac | 0:dc3a10b60a56 | 116 | //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000}; |
kikoaac | 0:dc3a10b60a56 | 117 | //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 }; |
kikoaac | 0:dc3a10b60a56 | 118 | //AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)}; |
kikoaac | 0:dc3a10b60a56 | 119 | Serial dev(D1,D0); |
kikoaac | 0:dc3a10b60a56 | 120 | Serial sbdbt(D13,D12); |
kikoaac | 0:dc3a10b60a56 | 121 | #define dataNum 12 |
kikoaac | 0:dc3a10b60a56 | 122 | void waitTime(float ti){ |
kikoaac | 0:dc3a10b60a56 | 123 | Timer t; |
kikoaac | 0:dc3a10b60a56 | 124 | t.start(); |
kikoaac | 0:dc3a10b60a56 | 125 | while(ti > t.read()); |
kikoaac | 0:dc3a10b60a56 | 126 | t.stop(); |
kikoaac | 0:dc3a10b60a56 | 127 | return; |
kikoaac | 0:dc3a10b60a56 | 128 | } |
kikoaac | 0:dc3a10b60a56 | 129 | Timer timer; |
kikoaac | 0:dc3a10b60a56 | 130 | char *tmp[2]; |
kikoaac | 0:dc3a10b60a56 | 131 | char RXData[dataNum] = {'0'}; |
kikoaac | 0:dc3a10b60a56 | 132 | void RX(){ |
kikoaac | 0:dc3a10b60a56 | 133 | if(dev.getc() == '0'){ |
kikoaac | 0:dc3a10b60a56 | 134 | timer.reset(); |
kikoaac | 0:dc3a10b60a56 | 135 | debugLed1 = true; |
kikoaac | 0:dc3a10b60a56 | 136 | for(int i = 1 ; i < dataNum;i++){ |
kikoaac | 0:dc3a10b60a56 | 137 | RXData[i] = dev.getc(); |
kikoaac | 0:dc3a10b60a56 | 138 | } |
kikoaac | 0:dc3a10b60a56 | 139 | //if(DEBUG && !DEBUG_R)sbdbt.printf("L:"); |
kikoaac | 0:dc3a10b60a56 | 140 | for(int i = 0 ;i < 4 ; i++){ |
kikoaac | 0:dc3a10b60a56 | 141 | floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2]; |
kikoaac | 0:dc3a10b60a56 | 142 | in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2]; |
kikoaac | 0:dc3a10b60a56 | 143 | in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2]; |
kikoaac | 0:dc3a10b60a56 | 144 | uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff); |
kikoaac | 0:dc3a10b60a56 | 145 | //uint16_t intt = map(in_,ReverseL[i] == true ? 0xffff - MaxmumRangeL[i] : MinimumRangeL[i],ReverseL[i] == true ? 0xffff - MinimumRangeL[i] : MaxmumRangeL[i],0,65535); |
kikoaac | 0:dc3a10b60a56 | 146 | uint16_t intt = map(in_,MinimumRangeL[i],MaxmumRangeL[i],0,65535); |
kikoaac | 0:dc3a10b60a56 | 147 | #if !CALIB |
kikoaac | 0:dc3a10b60a56 | 148 | intt = ReverseL[i] == true ? 0xffff - intt : intt; |
kikoaac | 0:dc3a10b60a56 | 149 | #endif |
kikoaac | 0:dc3a10b60a56 | 150 | floatInByte intt_; |
kikoaac | 0:dc3a10b60a56 | 151 | intt_.si = intt; |
kikoaac | 0:dc3a10b60a56 | 152 | //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d ",intt); |
kikoaac | 0:dc3a10b60a56 | 153 | //uint16_t intt = map(in_,13107,52428,0,65535); |
kikoaac | 0:dc3a10b60a56 | 154 | RXData[4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕 |
kikoaac | 0:dc3a10b60a56 | 155 | RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕 |
kikoaac | 0:dc3a10b60a56 | 156 | }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n"); |
kikoaac | 0:dc3a10b60a56 | 157 | |
kikoaac | 0:dc3a10b60a56 | 158 | timer.reset(); |
kikoaac | 0:dc3a10b60a56 | 159 | send = true; |
kikoaac | 0:dc3a10b60a56 | 160 | } |
kikoaac | 0:dc3a10b60a56 | 161 | } |
kikoaac | 0:dc3a10b60a56 | 162 | void print(int N,char RXdata[12]){ |
kikoaac | 0:dc3a10b60a56 | 163 | floatInByte data; |
kikoaac | 0:dc3a10b60a56 | 164 | for(int i = 0 ; i < 2 ; i ++){ |
kikoaac | 0:dc3a10b60a56 | 165 | data.c[i] = RXdata[N+i]; |
kikoaac | 0:dc3a10b60a56 | 166 | } |
kikoaac | 0:dc3a10b60a56 | 167 | sbdbt.printf("%d ",data.si); |
kikoaac | 0:dc3a10b60a56 | 168 | } |
kikoaac | 0:dc3a10b60a56 | 169 | double offset[4] = {0,0,0,-132}; |
kikoaac | 0:dc3a10b60a56 | 170 | double range[4] = {120,120,90,240}; |
kikoaac | 0:dc3a10b60a56 | 171 | double range2[4] = {120,120,90,9}; |
kikoaac | 0:dc3a10b60a56 | 172 | int main() { |
kikoaac | 0:dc3a10b60a56 | 173 | |
kikoaac | 0:dc3a10b60a56 | 174 | dev.baud(115200); |
kikoaac | 0:dc3a10b60a56 | 175 | |
kikoaac | 0:dc3a10b60a56 | 176 | sbdbt.baud(115200); |
kikoaac | 0:dc3a10b60a56 | 177 | for(int i = 0 ; i < 2; i++) |
kikoaac | 0:dc3a10b60a56 | 178 | { |
kikoaac | 0:dc3a10b60a56 | 179 | tmp[i] = new char[dataNum]; |
kikoaac | 0:dc3a10b60a56 | 180 | } |
kikoaac | 0:dc3a10b60a56 | 181 | debugLed1 = true; |
kikoaac | 0:dc3a10b60a56 | 182 | for(int i = 0 ; i < 10 ; i ++ ){ |
kikoaac | 0:dc3a10b60a56 | 183 | debugLed2 = !debugLed2; |
kikoaac | 0:dc3a10b60a56 | 184 | wait(0.01); |
kikoaac | 0:dc3a10b60a56 | 185 | } |
kikoaac | 0:dc3a10b60a56 | 186 | wait(0.5); |
kikoaac | 0:dc3a10b60a56 | 187 | dev.attach(RX, Serial::RxIrq); |
kikoaac | 0:dc3a10b60a56 | 188 | dev.putc('L'); |
kikoaac | 0:dc3a10b60a56 | 189 | timer.start(); |
kikoaac | 0:dc3a10b60a56 | 190 | int count = 0; |
kikoaac | 0:dc3a10b60a56 | 191 | while(1) { |
kikoaac | 0:dc3a10b60a56 | 192 | //送信データ格納 |
kikoaac | 0:dc3a10b60a56 | 193 | tmp[R][0] = 'H'; |
kikoaac | 0:dc3a10b60a56 | 194 | tmp[R][1] = 0; |
kikoaac | 0:dc3a10b60a56 | 195 | tmp[R][2] = 0; |
kikoaac | 0:dc3a10b60a56 | 196 | tmp[R][3] = 0; |
kikoaac | 0:dc3a10b60a56 | 197 | |
kikoaac | 0:dc3a10b60a56 | 198 | for(int i = 0 ;i < 4 ; i++){ |
kikoaac | 0:dc3a10b60a56 | 199 | floatInByte intt_; |
kikoaac | 0:dc3a10b60a56 | 200 | uint16_t in = ArmSense[i].read_u16(); |
kikoaac | 0:dc3a10b60a56 | 201 | uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535); |
kikoaac | 0:dc3a10b60a56 | 202 | #if !CALIB |
kikoaac | 0:dc3a10b60a56 | 203 | intt = ReverseR[i] == true ? 0xffff - intt : intt; |
kikoaac | 0:dc3a10b60a56 | 204 | #endif |
kikoaac | 0:dc3a10b60a56 | 205 | intt_.si = intt; |
kikoaac | 0:dc3a10b60a56 | 206 | tmp[R][4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕 |
kikoaac | 0:dc3a10b60a56 | 207 | tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕 |
kikoaac | 0:dc3a10b60a56 | 208 | } |
kikoaac | 0:dc3a10b60a56 | 209 | //if(DEBUG && DEBUG_R)sbdbt.printf("\n"); |
kikoaac | 0:dc3a10b60a56 | 210 | tmp[L] = RXData; |
kikoaac | 0:dc3a10b60a56 | 211 | char** SerialData = tmp; |
kikoaac | 0:dc3a10b60a56 | 212 | |
kikoaac | 0:dc3a10b60a56 | 213 | //送信データを送る |
kikoaac | 0:dc3a10b60a56 | 214 | //SerialData = tmp; |
kikoaac | 0:dc3a10b60a56 | 215 | if(count > 20 && send == true){ |
kikoaac | 0:dc3a10b60a56 | 216 | #if CALIB |
kikoaac | 0:dc3a10b60a56 | 217 | sbdbt.printf("R:"); |
kikoaac | 0:dc3a10b60a56 | 218 | for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]); |
kikoaac | 0:dc3a10b60a56 | 219 | sbdbt.printf("L:"); |
kikoaac | 0:dc3a10b60a56 | 220 | for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]); |
kikoaac | 0:dc3a10b60a56 | 221 | #endif |
kikoaac | 0:dc3a10b60a56 | 222 | for(int j = 0; j < 2 ; j++){ |
kikoaac | 0:dc3a10b60a56 | 223 | for(int i = 0 ; i < dataNum ; i++){ |
kikoaac | 0:dc3a10b60a56 | 224 | //sbdbt.printf("%3d ",int8_t(SerialData[j][i])); |
kikoaac | 0:dc3a10b60a56 | 225 | #if !CALIB |
kikoaac | 0:dc3a10b60a56 | 226 | if(!DEBUG)sbdbt.putc(SerialData[j][i]); |
kikoaac | 0:dc3a10b60a56 | 227 | #endif |
kikoaac | 0:dc3a10b60a56 | 228 | } |
kikoaac | 0:dc3a10b60a56 | 229 | } |
kikoaac | 0:dc3a10b60a56 | 230 | count = 0; |
kikoaac | 0:dc3a10b60a56 | 231 | if(!DEBUG)sbdbt.printf("\n"); |
kikoaac | 0:dc3a10b60a56 | 232 | } |
kikoaac | 0:dc3a10b60a56 | 233 | dev.putc('L'); |
kikoaac | 0:dc3a10b60a56 | 234 | send = false; |
kikoaac | 0:dc3a10b60a56 | 235 | while(timer.read_ms() >= 2000){ |
kikoaac | 0:dc3a10b60a56 | 236 | debugLed2 = true; |
kikoaac | 0:dc3a10b60a56 | 237 | waitTime(0.1); |
kikoaac | 0:dc3a10b60a56 | 238 | debugLed2 = false; |
kikoaac | 0:dc3a10b60a56 | 239 | waitTime(0.1); |
kikoaac | 0:dc3a10b60a56 | 240 | } |
kikoaac | 0:dc3a10b60a56 | 241 | debugLed1 = false; |
kikoaac | 0:dc3a10b60a56 | 242 | waitTime(1.0/1000); |
kikoaac | 0:dc3a10b60a56 | 243 | count ++; |
kikoaac | 0:dc3a10b60a56 | 244 | } |
kikoaac | 0:dc3a10b60a56 | 245 | } |
kikoaac | 0:dc3a10b60a56 | 246 | uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) { |
kikoaac | 0:dc3a10b60a56 | 247 | // check it's within the range |
kikoaac | 0:dc3a10b60a56 | 248 | if (inMin<inMax) { |
kikoaac | 0:dc3a10b60a56 | 249 | if (in <= inMin) |
kikoaac | 0:dc3a10b60a56 | 250 | return outMin; |
kikoaac | 0:dc3a10b60a56 | 251 | if (in >= inMax) |
kikoaac | 0:dc3a10b60a56 | 252 | return outMax; |
kikoaac | 0:dc3a10b60a56 | 253 | } else { // cope with input range being backwards. |
kikoaac | 0:dc3a10b60a56 | 254 | if (in >= inMin) |
kikoaac | 0:dc3a10b60a56 | 255 | return outMin; |
kikoaac | 0:dc3a10b60a56 | 256 | if (in <= inMax) |
kikoaac | 0:dc3a10b60a56 | 257 | return outMax; |
kikoaac | 0:dc3a10b60a56 | 258 | } |
kikoaac | 0:dc3a10b60a56 | 259 | // calculate how far into the range we are |
kikoaac | 0:dc3a10b60a56 | 260 | float scale = float(in-inMin)/float(inMax-inMin); |
kikoaac | 0:dc3a10b60a56 | 261 | // calculate the output. |
kikoaac | 0:dc3a10b60a56 | 262 | return uint16_t(outMin + scale*float(outMax-outMin)); |
kikoaac | 0:dc3a10b60a56 | 263 | } |