ken fuji / Mbed 2 deprecated robocon2017mbed_contoroler_L2

修正

Dependencies:   MyLib mbed

Fork of robocon2017mbed_contoroler_L2 by Kiko Ishimoto

main.cpp@3:21fb6b67bb54, 2017-10-19 (annotated)

Committer:
kikoaac
Date:
Thu Oct 19 13:41:29 2017 +0000
Revision:
3:21fb6b67bb54
Parent:
2:5d86b09dd7d8
Child:
4:10a3b9d4d219
???;

Who changed what in which revision?

UserRevisionLine numberNew 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 2:5d86b09dd7d8 40 nowAnalog = float(short(nowAnalog*0xFFFF)&0xFF00)/0xFFFF;
kikoaac 0:84122dbed53a 41 prevAnalog = nowAnalog;
kikoaac 0:84122dbed53a 42 return short(nowAnalog*0xFFFF);
kikoaac 0:84122dbed53a 43 }
kikoaac 0:84122dbed53a 44 };
kikoaac 0:84122dbed53a 45 class InLPF
kikoaac 0:84122dbed53a 46 {
kikoaac 0:84122dbed53a 47 private:
kikoaac 0:84122dbed53a 48 float alpha;
kikoaac 0:84122dbed53a 49 float prevAnalog;
kikoaac 0:84122dbed53a 50 float nowAnalog;
kikoaac 0:84122dbed53a 51 public : InLPF(float alpha_ = 0.2)
kikoaac 0:84122dbed53a 52 {
kikoaac 0:84122dbed53a 53 alpha = alpha_;
kikoaac 0:84122dbed53a 54 prevAnalog = 0.0;
kikoaac 0:84122dbed53a 55 }
kikoaac 0:84122dbed53a 56 float read(float in){
kikoaac 0:84122dbed53a 57 nowAnalog = in;
kikoaac 0:84122dbed53a 58 nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
kikoaac 0:84122dbed53a 59 prevAnalog = nowAnalog;
kikoaac 0:84122dbed53a 60 return nowAnalog;
kikoaac 0:84122dbed53a 61 }
kikoaac 0:84122dbed53a 62 short read_u16(float in){
kikoaac 0:84122dbed53a 63 nowAnalog = in;
kikoaac 0:84122dbed53a 64 nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
kikoaac 0:84122dbed53a 65 prevAnalog = nowAnalog;
kikoaac 2:5d86b09dd7d8 66 return short(nowAnalog*0xFFFF)&0xFF00;
kikoaac 0:84122dbed53a 67 }
kikoaac 0:84122dbed53a 68 };
kikoaac 0:84122dbed53a 69 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax);
kikoaac 2:5d86b09dd7d8 70 #define LP 0.30
kikoaac 2:5d86b09dd7d8 71 AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP-0.018),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP-0.018),AnalogInLPF(A3,LP)};
kikoaac 0:84122dbed53a 72 InLPF ArmSense2[4] = {InLPF(LP),InLPF(LP),InLPF(LP),InLPF(LP)};
kikoaac 0:84122dbed53a 73 #if !CALIB
kikoaac 0:84122dbed53a 74 //1号機
kikoaac 3:21fb6b67bb54 75
kikoaac 3:21fb6b67bb54 76 uint16_t MinimumRangeR[4] = {1280,41414,15872,34304};
kikoaac 3:21fb6b67bb54 77 uint16_t MaxmumRangeR[4] = {45312,52242,54272,11776};
kikoaac 3:21fb6b67bb54 78 uint16_t MinimumRangeL[4] = {23040,18432,24576,58112};
kikoaac 3:21fb6b67bb54 79 uint16_t MaxmumRangeL[4] = {45312,58112,59392,30976};
kikoaac 3:21fb6b67bb54 80
kikoaac 2:5d86b09dd7d8 81 /*
kikoaac 3:21fb6b67bb54 82 uint16_t MinimumRangeR[4] = {1280,42240,36096,17152};
kikoaac 3:21fb6b67bb54 83 uint16_t MaxmumRangeR[4] = {45312,52992,58624,41216};
kikoaac 3:21fb6b67bb54 84 uint16_t MinimumRangeL[4] = {23040,18432,24576,58112};
kikoaac 3:21fb6b67bb54 85 uint16_t MaxmumRangeL[4] = {45312,58112,59392,30976};
kikoaac 3:21fb6b67bb54 86 */
kikoaac 0:84122dbed53a 87 //2号機
kikoaac 3:21fb6b67bb54 88 /*
kikoaac 2:5d86b09dd7d8 89 uint16_t MinimumRangeR[4] = {19900,38656,36096,17152};
kikoaac 2:5d86b09dd7d8 90 uint16_t MaxmumRangeR[4] = {43260,53760,58624,41216};
kikoaac 2:5d86b09dd7d8 91 uint16_t MinimumRangeL[4] = {21500,19400,8448,57088};
kikoaac 2:5d86b09dd7d8 92 uint16_t MaxmumRangeL[4] = {43520,38900,38400,30208};
kikoaac 3:21fb6b67bb54 93 */
kikoaac 0:84122dbed53a 94 #endif
kikoaac 0:84122dbed53a 95 #if CALIB
kikoaac 0:84122dbed53a 96 uint16_t MinimumRangeR[4] = {0,0,0,0};
kikoaac 0:84122dbed53a 97 uint16_t MaxmumRangeR[4] = {0xffff,0xffff,0xffff,0xffff};
kikoaac 0:84122dbed53a 98 uint16_t MinimumRangeL[4] = {0,0,0,0};
kikoaac 0:84122dbed53a 99 uint16_t MaxmumRangeL[4] = {0xffff,0xffff,0xffff,0xffff};
kikoaac 0:84122dbed53a 100 #endif
kikoaac 0:84122dbed53a 101 //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
kikoaac 0:84122dbed53a 102 //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
kikoaac 0:84122dbed53a 103 bool ReverseL[4] = {false,true,true,true};
kikoaac 2:5d86b09dd7d8 104 bool ReverseR[4] = {true,false,true,false};
kikoaac 0:84122dbed53a 105 //AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)};
kikoaac 0:84122dbed53a 106 Nunchuck ctrl(D4,D5);
kikoaac 0:84122dbed53a 107 Serial dev(D1,D0);
kikoaac 0:84122dbed53a 108 Serial sbdbt(D13,D12);
kikoaac 0:84122dbed53a 109 #define dataNum 12
kikoaac 0:84122dbed53a 110 void waitTime(float ti){
kikoaac 0:84122dbed53a 111 Timer t;
kikoaac 0:84122dbed53a 112 t.start();
kikoaac 0:84122dbed53a 113 while(ti > t.read());
kikoaac 0:84122dbed53a 114 t.stop();
kikoaac 0:84122dbed53a 115 return;
kikoaac 0:84122dbed53a 116 }
kikoaac 0:84122dbed53a 117 Timer timer;
kikoaac 0:84122dbed53a 118 char *tmp[2];
kikoaac 0:84122dbed53a 119 char RXData[dataNum] = {'0'};
kikoaac 0:84122dbed53a 120 void RX(){
kikoaac 0:84122dbed53a 121 if(dev.getc() == '0'){
kikoaac 0:84122dbed53a 122 timer.reset();
kikoaac 0:84122dbed53a 123 debugLed1 = true;
kikoaac 0:84122dbed53a 124 for(int i = 1 ; i < dataNum;i++){
kikoaac 0:84122dbed53a 125 RXData[i] = dev.getc();
kikoaac 0:84122dbed53a 126 }
kikoaac 0:84122dbed53a 127 //if(DEBUG && !DEBUG_R)sbdbt.printf("L:");
kikoaac 0:84122dbed53a 128 for(int i = 0 ;i < 4 ; i++){
kikoaac 0:84122dbed53a 129 floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2];
kikoaac 0:84122dbed53a 130 in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2];
kikoaac 0:84122dbed53a 131 in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2];
kikoaac 0:84122dbed53a 132 uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff);
kikoaac 2:5d86b09dd7d8 133 //uint16_t intt = map(in_,ReverseL[i] == true ? 0xffff - MaxmumRangeL[i] : MinimumRangeL[i],ReverseL[i] == true ? 0xffff - MinimumRangeL[i] : MaxmumRangeL[i],0,65535);
kikoaac 2:5d86b09dd7d8 134 uint16_t intt = map(in_,MinimumRangeL[i],MaxmumRangeL[i],0,65535);
kikoaac 2:5d86b09dd7d8 135 #if !CALIB
kikoaac 0:84122dbed53a 136 intt = ReverseL[i] == true ? 0xffff - intt : intt;
kikoaac 2:5d86b09dd7d8 137 #endif
kikoaac 0:84122dbed53a 138 floatInByte intt_;
kikoaac 0:84122dbed53a 139 intt_.si = intt;
kikoaac 0:84122dbed53a 140 //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d ",intt);
kikoaac 0:84122dbed53a 141 //uint16_t intt = map(in_,13107,52428,0,65535);
kikoaac 0:84122dbed53a 142 RXData[4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕
kikoaac 0:84122dbed53a 143 RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕
kikoaac 0:84122dbed53a 144 }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n");
kikoaac 0:84122dbed53a 145
kikoaac 0:84122dbed53a 146 timer.reset();
kikoaac 0:84122dbed53a 147 send = true;
kikoaac 0:84122dbed53a 148 }
kikoaac 0:84122dbed53a 149 }
kikoaac 0:84122dbed53a 150 void print(int N,char RXdata[12]){
kikoaac 0:84122dbed53a 151 floatInByte data;
kikoaac 0:84122dbed53a 152 for(int i = 0 ; i < 2 ; i ++){
kikoaac 0:84122dbed53a 153 data.c[i] = RXdata[N+i];
kikoaac 0:84122dbed53a 154 }
kikoaac 0:84122dbed53a 155 sbdbt.printf("%d ",data.si);
kikoaac 0:84122dbed53a 156 }
kikoaac 0:84122dbed53a 157 double offset[4] = {0,0,0,-132};
kikoaac 0:84122dbed53a 158 double range[4] = {120,120,90,240};
kikoaac 0:84122dbed53a 159 double range2[4] = {120,120,90,9};
kikoaac 0:84122dbed53a 160 int main() {
kikoaac 0:84122dbed53a 161
kikoaac 0:84122dbed53a 162 dev.baud(115200);
kikoaac 0:84122dbed53a 163
kikoaac 0:84122dbed53a 164 sbdbt.baud(115200);
kikoaac 0:84122dbed53a 165 for(int i = 0 ; i < 2; i++)
kikoaac 0:84122dbed53a 166 {
kikoaac 0:84122dbed53a 167 tmp[i] = new char[dataNum];
kikoaac 0:84122dbed53a 168 }
kikoaac 0:84122dbed53a 169 debugLed1 = true;
kikoaac 0:84122dbed53a 170 for(int i = 0 ; i < 50 ; i ++ ){
kikoaac 0:84122dbed53a 171 debugLed2 = !debugLed2;
kikoaac 0:84122dbed53a 172 wait(0.1);
kikoaac 0:84122dbed53a 173 }
kikoaac 0:84122dbed53a 174 dev.attach(RX, Serial::RxIrq);
kikoaac 0:84122dbed53a 175 dev.putc('L');
kikoaac 0:84122dbed53a 176 timer.start();
kikoaac 0:84122dbed53a 177 int count = 0;
kikoaac 0:84122dbed53a 178 while(1) {
kikoaac 0:84122dbed53a 179 //送信データ格納
kikoaac 0:84122dbed53a 180
kikoaac 0:84122dbed53a 181
kikoaac 0:84122dbed53a 182 tmp[R][0] = 'H';
kikoaac 0:84122dbed53a 183 tmp[R][1] = ctrl.analogx();//ヌンチャクアナログX
kikoaac 0:84122dbed53a 184 tmp[R][2] = ctrl.analogy();//ヌンチャクアナログy
kikoaac 0:84122dbed53a 185 tmp[R][3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン
kikoaac 0:84122dbed53a 186 for(int i = 0 ;i < 4 ; i++){
kikoaac 0:84122dbed53a 187 floatInByte intt_;
kikoaac 0:84122dbed53a 188 //if(i==0 && DEBUG && DEBUG_R)sbdbt.printf("R:");
kikoaac 0:84122dbed53a 189 uint16_t in = ArmSense[i].read_u16();
kikoaac 0:84122dbed53a 190 //if(DEBUG && DEBUG_R)sbdbt.printf(" %5d ",intt);
kikoaac 2:5d86b09dd7d8 191 //uint16_t intt = map(in, ReverseR[i] == true ? 0xffff - MaxmumRangeR[i] : MinimumRangeR[i],ReverseR[i] == true ? 0xffff - MinimumRangeR[i] : MaxmumRangeR[i],0,65535);
kikoaac 2:5d86b09dd7d8 192 uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
kikoaac 2:5d86b09dd7d8 193 #if !CALIB
kikoaac 0:84122dbed53a 194 intt = ReverseR[i] == true ? 0xffff - intt : intt;
kikoaac 2:5d86b09dd7d8 195 #endif
kikoaac 0:84122dbed53a 196 intt_.si = intt;
kikoaac 0:84122dbed53a 197 //uint16_t intt = map(in_,13107,52428,0,65535);
kikoaac 0:84122dbed53a 198 tmp[R][4 + i*2] = 0;//intt_.c[0];//uint8_t(intt>>8);//マスター片腕
kikoaac 0:84122dbed53a 199 tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕
kikoaac 0:84122dbed53a 200 }
kikoaac 0:84122dbed53a 201 //if(DEBUG && DEBUG_R)sbdbt.printf("\n");
kikoaac 0:84122dbed53a 202 tmp[L] = RXData;
kikoaac 0:84122dbed53a 203 char** SerialData = tmp;
kikoaac 0:84122dbed53a 204
kikoaac 0:84122dbed53a 205 //送信データを送る
kikoaac 0:84122dbed53a 206 //SerialData = tmp;
kikoaac 3:21fb6b67bb54 207 if(count > 25 && send == true){
kikoaac 2:5d86b09dd7d8 208 /*for(int j = 0; j < 2 ; j++){
kikoaac 0:84122dbed53a 209 for(int i = 0 ; i < dataNum ; i++){
kikoaac 0:84122dbed53a 210 //if(!DEBUG)sbdbt.printf("%3d ",SerialData[j][i]);
kikoaac 0:84122dbed53a 211 //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
kikoaac 0:84122dbed53a 212 }
kikoaac 2:5d86b09dd7d8 213 }*/
kikoaac 3:21fb6b67bb54 214
kikoaac 2:5d86b09dd7d8 215 sbdbt.printf("R:");
kikoaac 0:84122dbed53a 216 for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]);
kikoaac 0:84122dbed53a 217 sbdbt.printf("L:");
kikoaac 0:84122dbed53a 218 for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]);
kikoaac 3:21fb6b67bb54 219
kikoaac 0:84122dbed53a 220 for(int j = 0; j < 2 ; j++){
kikoaac 0:84122dbed53a 221 for(int i = 0 ; i < dataNum ; i++){
kikoaac 0:84122dbed53a 222 //sbdbt.printf("%3d ",SerialData[j][i]);
kikoaac 3:21fb6b67bb54 223 //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
kikoaac 0:84122dbed53a 224 }
kikoaac 0:84122dbed53a 225 }
kikoaac 0:84122dbed53a 226 count = 0;
kikoaac 0:84122dbed53a 227 if(!DEBUG)sbdbt.printf("\n");
kikoaac 0:84122dbed53a 228 }
kikoaac 0:84122dbed53a 229 dev.putc('L');
kikoaac 0:84122dbed53a 230 send = false;
kikoaac 0:84122dbed53a 231 while(timer.read_ms() >= 2000){
kikoaac 0:84122dbed53a 232 debugLed2 = true;
kikoaac 0:84122dbed53a 233 waitTime(0.1);
kikoaac 0:84122dbed53a 234 debugLed2 = false;
kikoaac 0:84122dbed53a 235 waitTime(0.1);
kikoaac 0:84122dbed53a 236 }
kikoaac 0:84122dbed53a 237 debugLed1 = false;
kikoaac 2:5d86b09dd7d8 238 waitTime(0.0005);
kikoaac 0:84122dbed53a 239 count ++;
kikoaac 0:84122dbed53a 240 #if DEBUG
kikoaac 0:84122dbed53a 241 int RL = R;
kikoaac 0:84122dbed53a 242 if(DEBUG_R){sbdbt.printf("R:");RL = R;}
kikoaac 0:84122dbed53a 243 if(!DEBUG_R){sbdbt.printf("L:");RL = L;}
kikoaac 0:84122dbed53a 244 for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[RL]);
kikoaac 0:84122dbed53a 245 sbdbt.printf("\n");
kikoaac 0:84122dbed53a 246 #endif
kikoaac 0:84122dbed53a 247 }
kikoaac 0:84122dbed53a 248 }
kikoaac 0:84122dbed53a 249 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) {
kikoaac 0:84122dbed53a 250 // check it's within the range
kikoaac 0:84122dbed53a 251 if (inMin<inMax) {
kikoaac 0:84122dbed53a 252 if (in <= inMin)
kikoaac 0:84122dbed53a 253 return outMin;
kikoaac 0:84122dbed53a 254 if (in >= inMax)
kikoaac 0:84122dbed53a 255 return outMax;
kikoaac 0:84122dbed53a 256 } else { // cope with input range being backwards.
kikoaac 0:84122dbed53a 257 if (in >= inMin)
kikoaac 0:84122dbed53a 258 return outMin;
kikoaac 0:84122dbed53a 259 if (in <= inMax)
kikoaac 0:84122dbed53a 260 return outMax;
kikoaac 0:84122dbed53a 261 }
kikoaac 0:84122dbed53a 262 // calculate how far into the range we are
kikoaac 0:84122dbed53a 263 float scale = float(in-inMin)/float(inMax-inMin);
kikoaac 0:84122dbed53a 264 // calculate the output.
kikoaac 0:84122dbed53a 265 return uint16_t(outMin + scale*float(outMax-outMin));
kikoaac 0:84122dbed53a 266 }