ken fuji
/
robocon2017mbed_contoroler_L2
修正
Fork of robocon2017mbed_contoroler_L2 by
main.cpp@0:84122dbed53a, 2017-10-13 (annotated)
- Committer:
- kikoaac
- Date:
- Fri Oct 13 09:35:40 2017 +0000
- Revision:
- 0:84122dbed53a
- Child:
- 2:5d86b09dd7d8
???????;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kikoaac | 0:84122dbed53a | 1 | #include "mbed.h" |
kikoaac | 0:84122dbed53a | 2 | #include "Nunchuck.h" |
kikoaac | 0:84122dbed53a | 3 | #define R 0 |
kikoaac | 0:84122dbed53a | 4 | #define L 1 |
kikoaac | 0:84122dbed53a | 5 | |
kikoaac | 0:84122dbed53a | 6 | #define DEBUG 0 |
kikoaac | 0:84122dbed53a | 7 | #define DEBUG_R 1 |
kikoaac | 0:84122dbed53a | 8 | #define CALIB 0 |
kikoaac | 0:84122dbed53a | 9 | |
kikoaac | 0:84122dbed53a | 10 | #define SetupCMD 'A' |
kikoaac | 0:84122dbed53a | 11 | #define Respons 'S' |
kikoaac | 0:84122dbed53a | 12 | DigitalOut debugLed1(D6); |
kikoaac | 0:84122dbed53a | 13 | DigitalOut debugLed2(D7); |
kikoaac | 0:84122dbed53a | 14 | bool send = false; |
kikoaac | 0:84122dbed53a | 15 | union floatInByte |
kikoaac | 0:84122dbed53a | 16 | { |
kikoaac | 0:84122dbed53a | 17 | uint16_t si; |
kikoaac | 0:84122dbed53a | 18 | unsigned char c[2]; |
kikoaac | 0:84122dbed53a | 19 | }; |
kikoaac | 0:84122dbed53a | 20 | class AnalogInLPF : public AnalogIn |
kikoaac | 0:84122dbed53a | 21 | { |
kikoaac | 0:84122dbed53a | 22 | private: |
kikoaac | 0:84122dbed53a | 23 | float alpha; |
kikoaac | 0:84122dbed53a | 24 | float prevAnalog; |
kikoaac | 0:84122dbed53a | 25 | float nowAnalog; |
kikoaac | 0:84122dbed53a | 26 | public : AnalogInLPF(PinName pin,float alpha_) : AnalogIn(pin) |
kikoaac | 0:84122dbed53a | 27 | { |
kikoaac | 0:84122dbed53a | 28 | alpha = alpha_; |
kikoaac | 0:84122dbed53a | 29 | prevAnalog = 0.0; |
kikoaac | 0:84122dbed53a | 30 | } |
kikoaac | 0:84122dbed53a | 31 | float read(){ |
kikoaac | 0:84122dbed53a | 32 | nowAnalog = AnalogIn::read(); |
kikoaac | 0:84122dbed53a | 33 | nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; |
kikoaac | 0:84122dbed53a | 34 | prevAnalog = nowAnalog; |
kikoaac | 0:84122dbed53a | 35 | return nowAnalog; |
kikoaac | 0:84122dbed53a | 36 | } |
kikoaac | 0:84122dbed53a | 37 | short read_u16(){ |
kikoaac | 0:84122dbed53a | 38 | nowAnalog = AnalogIn::read(); |
kikoaac | 0:84122dbed53a | 39 | nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; |
kikoaac | 0:84122dbed53a | 40 | prevAnalog = nowAnalog; |
kikoaac | 0:84122dbed53a | 41 | return short(nowAnalog*0xFFFF); |
kikoaac | 0:84122dbed53a | 42 | } |
kikoaac | 0:84122dbed53a | 43 | }; |
kikoaac | 0:84122dbed53a | 44 | class InLPF |
kikoaac | 0:84122dbed53a | 45 | { |
kikoaac | 0:84122dbed53a | 46 | private: |
kikoaac | 0:84122dbed53a | 47 | float alpha; |
kikoaac | 0:84122dbed53a | 48 | float prevAnalog; |
kikoaac | 0:84122dbed53a | 49 | float nowAnalog; |
kikoaac | 0:84122dbed53a | 50 | public : InLPF(float alpha_ = 0.2) |
kikoaac | 0:84122dbed53a | 51 | { |
kikoaac | 0:84122dbed53a | 52 | alpha = alpha_; |
kikoaac | 0:84122dbed53a | 53 | prevAnalog = 0.0; |
kikoaac | 0:84122dbed53a | 54 | } |
kikoaac | 0:84122dbed53a | 55 | float read(float in){ |
kikoaac | 0:84122dbed53a | 56 | nowAnalog = in; |
kikoaac | 0:84122dbed53a | 57 | nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; |
kikoaac | 0:84122dbed53a | 58 | prevAnalog = nowAnalog; |
kikoaac | 0:84122dbed53a | 59 | return nowAnalog; |
kikoaac | 0:84122dbed53a | 60 | } |
kikoaac | 0:84122dbed53a | 61 | short read_u16(float in){ |
kikoaac | 0:84122dbed53a | 62 | nowAnalog = in; |
kikoaac | 0:84122dbed53a | 63 | nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; |
kikoaac | 0:84122dbed53a | 64 | prevAnalog = nowAnalog; |
kikoaac | 0:84122dbed53a | 65 | return short(nowAnalog*0xFFFF); |
kikoaac | 0:84122dbed53a | 66 | } |
kikoaac | 0:84122dbed53a | 67 | }; |
kikoaac | 0:84122dbed53a | 68 | uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax); |
kikoaac | 0:84122dbed53a | 69 | #define LP 0.5 |
kikoaac | 0:84122dbed53a | 70 | AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP),AnalogInLPF(A3,LP)}; |
kikoaac | 0:84122dbed53a | 71 | InLPF ArmSense2[4] = {InLPF(LP),InLPF(LP),InLPF(LP),InLPF(LP)}; |
kikoaac | 0:84122dbed53a | 72 | #if !CALIB |
kikoaac | 0:84122dbed53a | 73 | //1号機 |
kikoaac | 0:84122dbed53a | 74 | uint16_t MinimumRangeR[4] = {25700,39300,16400,53000}; |
kikoaac | 0:84122dbed53a | 75 | uint16_t MaxmumRangeR[4] = {44000,52600,62300,27600}; |
kikoaac | 0:84122dbed53a | 76 | uint16_t MinimumRangeL[4] = {20600,28500,2114,33300}; |
kikoaac | 0:84122dbed53a | 77 | uint16_t MaxmumRangeL[4] = {44000,43300,58900,6021}; |
kikoaac | 0:84122dbed53a | 78 | //2号機 |
kikoaac | 0:84122dbed53a | 79 | /* |
kikoaac | 0:84122dbed53a | 80 | uint16_t MinimumRangeR[4] = {11900,32600,52100,0xffff-37800}; |
kikoaac | 0:84122dbed53a | 81 | uint16_t MaxmumRangeR[4] = {54500,48000,56000,0xffff-8900}; |
kikoaac | 0:84122dbed53a | 82 | uint16_t MinimumRangeL[4] = {62900,11300,37700,14200}; |
kikoaac | 0:84122dbed53a | 83 | uint16_t MaxmumRangeL[4] = {45800,56000,27900,44000}; |
kikoaac | 0:84122dbed53a | 84 | */ |
kikoaac | 0:84122dbed53a | 85 | #endif |
kikoaac | 0:84122dbed53a | 86 | #if CALIB |
kikoaac | 0:84122dbed53a | 87 | uint16_t MinimumRangeR[4] = {0,0,0,0}; |
kikoaac | 0:84122dbed53a | 88 | uint16_t MaxmumRangeR[4] = {0xffff,0xffff,0xffff,0xffff}; |
kikoaac | 0:84122dbed53a | 89 | uint16_t MinimumRangeL[4] = {0,0,0,0}; |
kikoaac | 0:84122dbed53a | 90 | uint16_t MaxmumRangeL[4] = {0xffff,0xffff,0xffff,0xffff}; |
kikoaac | 0:84122dbed53a | 91 | #endif |
kikoaac | 0:84122dbed53a | 92 | //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000}; |
kikoaac | 0:84122dbed53a | 93 | //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 }; |
kikoaac | 0:84122dbed53a | 94 | bool ReverseL[4] = {false,true,true,true}; |
kikoaac | 0:84122dbed53a | 95 | bool ReverseR[4] = {true,false,true,true}; |
kikoaac | 0:84122dbed53a | 96 | //AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)}; |
kikoaac | 0:84122dbed53a | 97 | Nunchuck ctrl(D4,D5); |
kikoaac | 0:84122dbed53a | 98 | Serial dev(D1,D0); |
kikoaac | 0:84122dbed53a | 99 | Serial sbdbt(D13,D12); |
kikoaac | 0:84122dbed53a | 100 | #define dataNum 12 |
kikoaac | 0:84122dbed53a | 101 | void waitTime(float ti){ |
kikoaac | 0:84122dbed53a | 102 | Timer t; |
kikoaac | 0:84122dbed53a | 103 | t.start(); |
kikoaac | 0:84122dbed53a | 104 | while(ti > t.read()); |
kikoaac | 0:84122dbed53a | 105 | t.stop(); |
kikoaac | 0:84122dbed53a | 106 | return; |
kikoaac | 0:84122dbed53a | 107 | } |
kikoaac | 0:84122dbed53a | 108 | Timer timer; |
kikoaac | 0:84122dbed53a | 109 | char *tmp[2]; |
kikoaac | 0:84122dbed53a | 110 | char RXData[dataNum] = {'0'}; |
kikoaac | 0:84122dbed53a | 111 | void RX(){ |
kikoaac | 0:84122dbed53a | 112 | if(dev.getc() == '0'){ |
kikoaac | 0:84122dbed53a | 113 | timer.reset(); |
kikoaac | 0:84122dbed53a | 114 | debugLed1 = true; |
kikoaac | 0:84122dbed53a | 115 | for(int i = 1 ; i < dataNum;i++){ |
kikoaac | 0:84122dbed53a | 116 | RXData[i] = dev.getc(); |
kikoaac | 0:84122dbed53a | 117 | } |
kikoaac | 0:84122dbed53a | 118 | //if(DEBUG && !DEBUG_R)sbdbt.printf("L:"); |
kikoaac | 0:84122dbed53a | 119 | for(int i = 0 ;i < 4 ; i++){ |
kikoaac | 0:84122dbed53a | 120 | floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2]; |
kikoaac | 0:84122dbed53a | 121 | in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2]; |
kikoaac | 0:84122dbed53a | 122 | in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2]; |
kikoaac | 0:84122dbed53a | 123 | uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff); |
kikoaac | 0:84122dbed53a | 124 | 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:84122dbed53a | 125 | intt = ReverseL[i] == true ? 0xffff - intt : intt; |
kikoaac | 0:84122dbed53a | 126 | floatInByte intt_; |
kikoaac | 0:84122dbed53a | 127 | intt_.si = intt; |
kikoaac | 0:84122dbed53a | 128 | //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d ",intt); |
kikoaac | 0:84122dbed53a | 129 | //uint16_t intt = map(in_,13107,52428,0,65535); |
kikoaac | 0:84122dbed53a | 130 | RXData[4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕 |
kikoaac | 0:84122dbed53a | 131 | RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕 |
kikoaac | 0:84122dbed53a | 132 | }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n"); |
kikoaac | 0:84122dbed53a | 133 | |
kikoaac | 0:84122dbed53a | 134 | timer.reset(); |
kikoaac | 0:84122dbed53a | 135 | send = true; |
kikoaac | 0:84122dbed53a | 136 | } |
kikoaac | 0:84122dbed53a | 137 | } |
kikoaac | 0:84122dbed53a | 138 | void print(int N,char RXdata[12]){ |
kikoaac | 0:84122dbed53a | 139 | floatInByte data; |
kikoaac | 0:84122dbed53a | 140 | for(int i = 0 ; i < 2 ; i ++){ |
kikoaac | 0:84122dbed53a | 141 | data.c[i] = RXdata[N+i]; |
kikoaac | 0:84122dbed53a | 142 | } |
kikoaac | 0:84122dbed53a | 143 | sbdbt.printf("%d ",data.si); |
kikoaac | 0:84122dbed53a | 144 | } |
kikoaac | 0:84122dbed53a | 145 | double offset[4] = {0,0,0,-132}; |
kikoaac | 0:84122dbed53a | 146 | double range[4] = {120,120,90,240}; |
kikoaac | 0:84122dbed53a | 147 | double range2[4] = {120,120,90,9}; |
kikoaac | 0:84122dbed53a | 148 | int main() { |
kikoaac | 0:84122dbed53a | 149 | |
kikoaac | 0:84122dbed53a | 150 | dev.baud(115200); |
kikoaac | 0:84122dbed53a | 151 | |
kikoaac | 0:84122dbed53a | 152 | sbdbt.baud(115200); |
kikoaac | 0:84122dbed53a | 153 | for(int i = 0 ; i < 2; i++) |
kikoaac | 0:84122dbed53a | 154 | { |
kikoaac | 0:84122dbed53a | 155 | tmp[i] = new char[dataNum]; |
kikoaac | 0:84122dbed53a | 156 | } |
kikoaac | 0:84122dbed53a | 157 | debugLed1 = true; |
kikoaac | 0:84122dbed53a | 158 | for(int i = 0 ; i < 50 ; i ++ ){ |
kikoaac | 0:84122dbed53a | 159 | debugLed2 = !debugLed2; |
kikoaac | 0:84122dbed53a | 160 | wait(0.1); |
kikoaac | 0:84122dbed53a | 161 | } |
kikoaac | 0:84122dbed53a | 162 | dev.attach(RX, Serial::RxIrq); |
kikoaac | 0:84122dbed53a | 163 | dev.putc('L'); |
kikoaac | 0:84122dbed53a | 164 | timer.start(); |
kikoaac | 0:84122dbed53a | 165 | int count = 0; |
kikoaac | 0:84122dbed53a | 166 | while(1) { |
kikoaac | 0:84122dbed53a | 167 | //送信データ格納 |
kikoaac | 0:84122dbed53a | 168 | |
kikoaac | 0:84122dbed53a | 169 | |
kikoaac | 0:84122dbed53a | 170 | tmp[R][0] = 'H'; |
kikoaac | 0:84122dbed53a | 171 | tmp[R][1] = ctrl.analogx();//ヌンチャクアナログX |
kikoaac | 0:84122dbed53a | 172 | tmp[R][2] = ctrl.analogy();//ヌンチャクアナログy |
kikoaac | 0:84122dbed53a | 173 | tmp[R][3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン |
kikoaac | 0:84122dbed53a | 174 | for(int i = 0 ;i < 4 ; i++){ |
kikoaac | 0:84122dbed53a | 175 | floatInByte intt_; |
kikoaac | 0:84122dbed53a | 176 | //if(i==0 && DEBUG && DEBUG_R)sbdbt.printf("R:"); |
kikoaac | 0:84122dbed53a | 177 | uint16_t in = ArmSense[i].read_u16(); |
kikoaac | 0:84122dbed53a | 178 | //if(DEBUG && DEBUG_R)sbdbt.printf(" %5d ",intt); |
kikoaac | 0:84122dbed53a | 179 | uint16_t intt = map(in, ReverseR[i] == true ? 0xffff - MaxmumRangeR[i] : MinimumRangeR[i],ReverseR[i] == true ? 0xffff - MinimumRangeR[i] : MaxmumRangeR[i],0,65535); |
kikoaac | 0:84122dbed53a | 180 | intt = ReverseR[i] == true ? 0xffff - intt : intt; |
kikoaac | 0:84122dbed53a | 181 | intt_.si = intt; |
kikoaac | 0:84122dbed53a | 182 | //uint16_t intt = map(in_,13107,52428,0,65535); |
kikoaac | 0:84122dbed53a | 183 | tmp[R][4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕 |
kikoaac | 0:84122dbed53a | 184 | tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕 |
kikoaac | 0:84122dbed53a | 185 | } |
kikoaac | 0:84122dbed53a | 186 | //if(DEBUG && DEBUG_R)sbdbt.printf("\n"); |
kikoaac | 0:84122dbed53a | 187 | tmp[L] = RXData; |
kikoaac | 0:84122dbed53a | 188 | char** SerialData = tmp; |
kikoaac | 0:84122dbed53a | 189 | |
kikoaac | 0:84122dbed53a | 190 | //送信データを送る |
kikoaac | 0:84122dbed53a | 191 | //SerialData = tmp; |
kikoaac | 0:84122dbed53a | 192 | if(count > 10 && send == true){ |
kikoaac | 0:84122dbed53a | 193 | for(int j = 0; j < 2 ; j++){ |
kikoaac | 0:84122dbed53a | 194 | for(int i = 0 ; i < dataNum ; i++){ |
kikoaac | 0:84122dbed53a | 195 | //if(!DEBUG)sbdbt.printf("%3d ",SerialData[j][i]); |
kikoaac | 0:84122dbed53a | 196 | //if(!DEBUG)sbdbt.putc(SerialData[j][i]); |
kikoaac | 0:84122dbed53a | 197 | } |
kikoaac | 0:84122dbed53a | 198 | } |
kikoaac | 0:84122dbed53a | 199 | /*sbdbt.printf("R:"); |
kikoaac | 0:84122dbed53a | 200 | for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]); |
kikoaac | 0:84122dbed53a | 201 | sbdbt.printf("L:"); |
kikoaac | 0:84122dbed53a | 202 | for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]); |
kikoaac | 0:84122dbed53a | 203 | */ |
kikoaac | 0:84122dbed53a | 204 | for(int j = 0; j < 2 ; j++){ |
kikoaac | 0:84122dbed53a | 205 | for(int i = 0 ; i < dataNum ; i++){ |
kikoaac | 0:84122dbed53a | 206 | //sbdbt.printf("%3d ",SerialData[j][i]); |
kikoaac | 0:84122dbed53a | 207 | if(!DEBUG)sbdbt.putc(SerialData[j][i]); |
kikoaac | 0:84122dbed53a | 208 | } |
kikoaac | 0:84122dbed53a | 209 | } |
kikoaac | 0:84122dbed53a | 210 | count = 0; |
kikoaac | 0:84122dbed53a | 211 | if(!DEBUG)sbdbt.printf("\n"); |
kikoaac | 0:84122dbed53a | 212 | } |
kikoaac | 0:84122dbed53a | 213 | dev.putc('L'); |
kikoaac | 0:84122dbed53a | 214 | send = false; |
kikoaac | 0:84122dbed53a | 215 | while(timer.read_ms() >= 2000){ |
kikoaac | 0:84122dbed53a | 216 | debugLed2 = true; |
kikoaac | 0:84122dbed53a | 217 | waitTime(0.1); |
kikoaac | 0:84122dbed53a | 218 | debugLed2 = false; |
kikoaac | 0:84122dbed53a | 219 | waitTime(0.1); |
kikoaac | 0:84122dbed53a | 220 | } |
kikoaac | 0:84122dbed53a | 221 | debugLed1 = false; |
kikoaac | 0:84122dbed53a | 222 | waitTime(0.001); |
kikoaac | 0:84122dbed53a | 223 | count ++; |
kikoaac | 0:84122dbed53a | 224 | #if DEBUG |
kikoaac | 0:84122dbed53a | 225 | int RL = R; |
kikoaac | 0:84122dbed53a | 226 | if(DEBUG_R){sbdbt.printf("R:");RL = R;} |
kikoaac | 0:84122dbed53a | 227 | if(!DEBUG_R){sbdbt.printf("L:");RL = L;} |
kikoaac | 0:84122dbed53a | 228 | for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[RL]); |
kikoaac | 0:84122dbed53a | 229 | sbdbt.printf("\n"); |
kikoaac | 0:84122dbed53a | 230 | #endif |
kikoaac | 0:84122dbed53a | 231 | } |
kikoaac | 0:84122dbed53a | 232 | } |
kikoaac | 0:84122dbed53a | 233 | uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) { |
kikoaac | 0:84122dbed53a | 234 | // check it's within the range |
kikoaac | 0:84122dbed53a | 235 | if (inMin<inMax) { |
kikoaac | 0:84122dbed53a | 236 | if (in <= inMin) |
kikoaac | 0:84122dbed53a | 237 | return outMin; |
kikoaac | 0:84122dbed53a | 238 | if (in >= inMax) |
kikoaac | 0:84122dbed53a | 239 | return outMax; |
kikoaac | 0:84122dbed53a | 240 | } else { // cope with input range being backwards. |
kikoaac | 0:84122dbed53a | 241 | if (in >= inMin) |
kikoaac | 0:84122dbed53a | 242 | return outMin; |
kikoaac | 0:84122dbed53a | 243 | if (in <= inMax) |
kikoaac | 0:84122dbed53a | 244 | return outMax; |
kikoaac | 0:84122dbed53a | 245 | } |
kikoaac | 0:84122dbed53a | 246 | // calculate how far into the range we are |
kikoaac | 0:84122dbed53a | 247 | float scale = float(in-inMin)/float(inMax-inMin); |
kikoaac | 0:84122dbed53a | 248 | // calculate the output. |
kikoaac | 0:84122dbed53a | 249 | return uint16_t(outMin + scale*float(outMax-outMin)); |
kikoaac | 0:84122dbed53a | 250 | } |