Kiko Ishimoto / Mbed 2 deprecated robocon2017mbed_controlLnew

Sd

Dependencies:   MyLibdd mbed

main.cpp@1:6ac2a4253dc2, 2018-01-14 (annotated)

Committer:
kikoaac
Date:
Sun Jan 14 01:09:34 2018 +0000
Revision:
1:6ac2a4253dc2
Parent:
0:eefd142a81de 
Lll;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 0:eefd142a81de 1 #if 0
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 uint16_t MinimumRangeR[4] = {0xffff-43000,48000,52100,0xffff-52000};
kikoaac 0:eefd142a81de 81 uint16_t MaxmumRangeR[4] = {0xffff-29800,53300,56000,0xffff-28400};
kikoaac 0:eefd142a81de 82 uint16_t MinimumRangeL[4] = {62900,11300,37700,14200};
kikoaac 0:eefd142a81de 83 uint16_t MaxmumRangeL[4] = {45800,56000,27900,44000};
kikoaac 0:eefd142a81de 84 #endif
kikoaac 0:eefd142a81de 85 #if CALIB
kikoaac 0:eefd142a81de 86 uint16_t MinimumRangeR[4] = {0,0,0,0};
kikoaac 0:eefd142a81de 87 uint16_t MaxmumRangeR[4] = {0xffff,0xffff,0xffff,0xffff};
kikoaac 0:eefd142a81de 88 uint16_t MinimumRangeL[4] = {0,0,0,0};
kikoaac 0:eefd142a81de 89 uint16_t MaxmumRangeL[4] = {0xffff,0xffff,0xffff,0xffff};
kikoaac 0:eefd142a81de 90 #endif
kikoaac 0:eefd142a81de 91 //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
kikoaac 0:eefd142a81de 92 //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
kikoaac 0:eefd142a81de 93 bool ReverseL[4] = {true,false,true,true};
kikoaac 0:eefd142a81de 94 bool ReverseR[4] = {true,false,false,false};
kikoaac 0:eefd142a81de 95 //AnalogIn ArmSense[4] = {AnalogIn(A6),AnalogIn(A5),AnalogIn(A4),AnalogIn(A3)};
kikoaac 0:eefd142a81de 96 Nunchuck ctrl(D4,D5);
kikoaac 0:eefd142a81de 97 Serial dev(D1,D0);
kikoaac 0:eefd142a81de 98 Serial sbdbt(D13,D12);
kikoaac 0:eefd142a81de 99 #define dataNum 12
kikoaac 0:eefd142a81de 100 void waitTime(float ti){
kikoaac 0:eefd142a81de 101 Timer t;
kikoaac 0:eefd142a81de 102 t.start();
kikoaac 0:eefd142a81de 103 while(ti > t.read());
kikoaac 0:eefd142a81de 104 t.stop();
kikoaac 0:eefd142a81de 105 return;
kikoaac 0:eefd142a81de 106 }
kikoaac 0:eefd142a81de 107 Timer timer;
kikoaac 0:eefd142a81de 108 char *tmp[2];
kikoaac 0:eefd142a81de 109 char RXData[dataNum] = {'0'};
kikoaac 0:eefd142a81de 110 void RX(){
kikoaac 0:eefd142a81de 111 if(dev.getc() == '0'){
kikoaac 0:eefd142a81de 112 timer.reset();
kikoaac 0:eefd142a81de 113 debugLed1 = true;
kikoaac 0:eefd142a81de 114 for(int i = 1 ; i < dataNum;i++){
kikoaac 0:eefd142a81de 115 RXData[i] = dev.getc();
kikoaac 0:eefd142a81de 116 }
kikoaac 0:eefd142a81de 117 //if(DEBUG && !DEBUG_R)sbdbt.printf("L:");
kikoaac 0:eefd142a81de 118 for(int i = 0 ;i < 4 ; i++){
kikoaac 0:eefd142a81de 119 floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2];
kikoaac 0:eefd142a81de 120 in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2];
kikoaac 0:eefd142a81de 121 in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2];
kikoaac 0:eefd142a81de 122 uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff);
kikoaac 0:eefd142a81de 123 uint16_t intt = map(in_,MinimumRangeL[i],MaxmumRangeL[i],0,65535);
kikoaac 0:eefd142a81de 124 intt = ReverseL[i] == true ? 0xffff - intt : intt;
kikoaac 0:eefd142a81de 125 floatInByte intt_;
kikoaac 0:eefd142a81de 126 intt_.si = intt;
kikoaac 0:eefd142a81de 127 //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d ",intt);
kikoaac 0:eefd142a81de 128 //uint16_t intt = map(in_,13107,52428,0,65535);
kikoaac 0:eefd142a81de 129 RXData[4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
kikoaac 0:eefd142a81de 130 RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕
kikoaac 0:eefd142a81de 131 }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n");
kikoaac 0:eefd142a81de 132
kikoaac 0:eefd142a81de 133 timer.reset();
kikoaac 0:eefd142a81de 134 send = true;
kikoaac 0:eefd142a81de 135 }
kikoaac 0:eefd142a81de 136 }
kikoaac 0:eefd142a81de 137 void print(int N,char RXdata[12]){
kikoaac 0:eefd142a81de 138 floatInByte data;
kikoaac 0:eefd142a81de 139 for(int i = 0 ; i < 2 ; i ++){
kikoaac 0:eefd142a81de 140 data.c[i] = RXdata[N+i];
kikoaac 0:eefd142a81de 141 }
kikoaac 0:eefd142a81de 142 sbdbt.printf("%d ",data.si);
kikoaac 0:eefd142a81de 143 }
kikoaac 0:eefd142a81de 144 double offset[4] = {0,0,0,-132};
kikoaac 0:eefd142a81de 145 double range[4] = {120,120,90,240};
kikoaac 0:eefd142a81de 146 double range2[4] = {120,120,90,9};
kikoaac 0:eefd142a81de 147 int main() {
kikoaac 0:eefd142a81de 148
kikoaac 0:eefd142a81de 149 dev.baud(115200);
kikoaac 0:eefd142a81de 150
kikoaac 0:eefd142a81de 151 sbdbt.baud(115200);
kikoaac 0:eefd142a81de 152 for(int i = 0 ; i < 2; i++)
kikoaac 0:eefd142a81de 153 {
kikoaac 0:eefd142a81de 154 tmp[i] = new char[dataNum];
kikoaac 0:eefd142a81de 155 }
kikoaac 0:eefd142a81de 156 debugLed1 = true;
kikoaac 0:eefd142a81de 157 for(int i = 0 ; i < 50 ; i ++ ){
kikoaac 0:eefd142a81de 158 debugLed2 = !debugLed2;
kikoaac 0:eefd142a81de 159 wait(0.1);
kikoaac 0:eefd142a81de 160 }
kikoaac 0:eefd142a81de 161 dev.attach(RX, Serial::RxIrq);
kikoaac 0:eefd142a81de 162 dev.putc('L');
kikoaac 0:eefd142a81de 163 timer.start();
kikoaac 0:eefd142a81de 164 while(1) {
kikoaac 0:eefd142a81de 165 //送信データ格納
kikoaac 0:eefd142a81de 166
kikoaac 0:eefd142a81de 167
kikoaac 0:eefd142a81de 168 tmp[R][0] = 'H';
kikoaac 0:eefd142a81de 169 tmp[R][1] = ctrl.analogx();//ヌンチャクアナログX
kikoaac 0:eefd142a81de 170 tmp[R][2] = ctrl.analogy();//ヌンチャクアナログy
kikoaac 0:eefd142a81de 171 tmp[R][3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン
kikoaac 0:eefd142a81de 172 for(int i = 0 ;i < 4 ; i++){
kikoaac 0:eefd142a81de 173 floatInByte intt_;
kikoaac 0:eefd142a81de 174 //if(i==0 && DEBUG && DEBUG_R)sbdbt.printf("R:");
kikoaac 0:eefd142a81de 175 uint16_t in = ArmSense[i].read_u16();
kikoaac 0:eefd142a81de 176 //if(DEBUG && DEBUG_R)sbdbt.printf(" %5d ",intt);
kikoaac 0:eefd142a81de 177 uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
kikoaac 0:eefd142a81de 178 intt = ReverseR[i] == true ? 0xffff - intt : intt;
kikoaac 0:eefd142a81de 179 intt_.si = intt;
kikoaac 0:eefd142a81de 180 //uint16_t intt = map(in_,13107,52428,0,65535);
kikoaac 0:eefd142a81de 181 tmp[R][4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
kikoaac 0:eefd142a81de 182 tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕
kikoaac 0:eefd142a81de 183 }
kikoaac 0:eefd142a81de 184 //if(DEBUG && DEBUG_R)sbdbt.printf("\n");
kikoaac 0:eefd142a81de 185 tmp[L] = RXData;
kikoaac 0:eefd142a81de 186 char** SerialData = tmp;
kikoaac 0:eefd142a81de 187
kikoaac 0:eefd142a81de 188 //送信データを送る
kikoaac 0:eefd142a81de 189 //SerialData = tmp;
kikoaac 0:eefd142a81de 190 if(send == true){
kikoaac 0:eefd142a81de 191 for(int j = 0; j < 2 ; j++){
kikoaac 0:eefd142a81de 192 for(int i = 0 ; i < dataNum ; i++){
kikoaac 0:eefd142a81de 193 //if(!DEBUG)sbdbt.printf("%3d ",SerialData[j][i]);
kikoaac 0:eefd142a81de 194 //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
kikoaac 0:eefd142a81de 195 }
kikoaac 0:eefd142a81de 196 }
kikoaac 0:eefd142a81de 197 sbdbt.printf("R:");
kikoaac 0:eefd142a81de 198 for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]);
kikoaac 0:eefd142a81de 199 sbdbt.printf("L:");
kikoaac 0:eefd142a81de 200 for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]);
kikoaac 0:eefd142a81de 201
kikoaac 0:eefd142a81de 202 for(int j = 0; j < 2 ; j++){
kikoaac 0:eefd142a81de 203 for(int i = 0 ; i < dataNum ; i++){
kikoaac 0:eefd142a81de 204 //sbdbt.printf("%3d ",SerialData[j][i]);
kikoaac 0:eefd142a81de 205 if(!DEBUG)sbdbt.putc(SerialData[j][i]);
kikoaac 0:eefd142a81de 206 }
kikoaac 0:eefd142a81de 207 }
kikoaac 0:eefd142a81de 208 if(!DEBUG)sbdbt.printf("\n");
kikoaac 0:eefd142a81de 209 }
kikoaac 0:eefd142a81de 210 dev.putc('L');
kikoaac 0:eefd142a81de 211 send = false;
kikoaac 0:eefd142a81de 212 while(timer.read_ms() >= 2000){
kikoaac 0:eefd142a81de 213 debugLed2 = true;
kikoaac 0:eefd142a81de 214 waitTime(0.1);
kikoaac 0:eefd142a81de 215 debugLed2 = false;
kikoaac 0:eefd142a81de 216 waitTime(0.1);
kikoaac 0:eefd142a81de 217 }
kikoaac 0:eefd142a81de 218 debugLed1 = false;
kikoaac 0:eefd142a81de 219 waitTime(0.01);
kikoaac 0:eefd142a81de 220 #if DEBUG
kikoaac 0:eefd142a81de 221 int RL = R;
kikoaac 0:eefd142a81de 222 if(DEBUG_R){sbdbt.printf("R:");RL = R;}
kikoaac 0:eefd142a81de 223 if(!DEBUG_R){sbdbt.printf("L:");RL = L;}
kikoaac 0:eefd142a81de 224 for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[RL]);
kikoaac 0:eefd142a81de 225 sbdbt.printf("\n");
kikoaac 0:eefd142a81de 226 #endif
kikoaac 0:eefd142a81de 227 }
kikoaac 0:eefd142a81de 228 }
kikoaac 0:eefd142a81de 229 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) {
kikoaac 0:eefd142a81de 230 // check it's within the range
kikoaac 0:eefd142a81de 231 if (inMin<inMax) {
kikoaac 0:eefd142a81de 232 if (in <= inMin)
kikoaac 0:eefd142a81de 233 return outMin;
kikoaac 0:eefd142a81de 234 if (in >= inMax)
kikoaac 0:eefd142a81de 235 return outMax;
kikoaac 0:eefd142a81de 236 } else { // cope with input range being backwards.
kikoaac 0:eefd142a81de 237 if (in >= inMin)
kikoaac 0:eefd142a81de 238 return outMin;
kikoaac 0:eefd142a81de 239 if (in <= inMax)
kikoaac 0:eefd142a81de 240 return outMax;
kikoaac 0:eefd142a81de 241 }
kikoaac 0:eefd142a81de 242 // calculate how far into the range we are
kikoaac 0:eefd142a81de 243 float scale = float(in-inMin)/float(inMax-inMin);
kikoaac 0:eefd142a81de 244 // calculate the output.
kikoaac 0:eefd142a81de 245 return uint16_t(outMin + scale*float(outMax-outMin));
kikoaac 0:eefd142a81de 246 }
kikoaac 0:eefd142a81de 247 #endif