Kiko Ishimoto / Mbed 2 deprecated robocon2017mbed_controlLnew

Sd

Dependencies:   MyLibdd mbed

main2.cpp@0:eefd142a81de, 2018-01-14 (annotated)

Committer:
kikoaac
Date:
Sun Jan 14 01:06:49 2018 +0000
Revision:
0:eefd142a81de
???

Who changed what in which revision?

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