Kiko Ishimoto / Mbed 2 deprecated robocon2017mbed_nedoControl_L

nedo用　未検証

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }