Kiko Ishimoto / Mbed 2 deprecated robocon2017mbed_nedoControl_Lplus

nedo用　手のひらきと腕を回す

Dependencies:   mbed

robocon2017mbed_nedoControl_Lplus.cpp@0:495b957db932, 2017-11-16 (annotated)

Committer:
kikoaac
Date:
Thu Nov 16 13:25:48 2017 +0000
Revision:
0:495b957db932
nedo;

Who changed what in which revision?

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