Kiko Ishimoto / Mbed 2 deprecated robocon2017mbed_controlerLmotion

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MyLib3 mbed

main.cpp@0:4a1a924f704e, 2017-10-25 (annotated)

Committer:
kikoaac
Date:
Wed Oct 25 10:58:54 2017 +0000
Revision:
0:4a1a924f704e
??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 0:4a1a924f704e 1 #include "mbed.h"
kikoaac 0:4a1a924f704e 2 char *tmp[2];
kikoaac 0:4a1a924f704e 3 char *SenseTmp[2];
kikoaac 0:4a1a924f704e 4 #include "Nunchuck.h"
kikoaac 0:4a1a924f704e 5 #include "defines.h"
kikoaac 0:4a1a924f704e 6 #include "param_motion.h"
kikoaac 0:4a1a924f704e 7 Serial dev(D1,D0);
kikoaac 0:4a1a924f704e 8 Serial sbdbt(D13,D12);
kikoaac 0:4a1a924f704e 9 #include "methods.h"
kikoaac 0:4a1a924f704e 10
kikoaac 0:4a1a924f704e 11 DigitalOut debugLed1(D6);
kikoaac 0:4a1a924f704e 12 DigitalOut debugLed2(D7);
kikoaac 0:4a1a924f704e 13 bool send = false;
kikoaac 0:4a1a924f704e 14
kikoaac 0:4a1a924f704e 15 //uint16_t MinimumRangeL[4] = {19000,35000,35600,21000};
kikoaac 0:4a1a924f704e 16 //uint16_t MaxmumRangeL[4] = {49000,57000,43000,42000 };
kikoaac 0:4a1a924f704e 17 bool ReverseL[4] = {true,false,true,true};
kikoaac 0:4a1a924f704e 18 bool ReverseR[4] = {false,false,false,true};
kikoaac 0:4a1a924f704e 19 Timer timer;
kikoaac 0:4a1a924f704e 20 void RX(){
kikoaac 0:4a1a924f704e 21 if(dev.getc() == '0'){
kikoaac 0:4a1a924f704e 22 timer.reset();
kikoaac 0:4a1a924f704e 23 debugLed1 = true;
kikoaac 0:4a1a924f704e 24 for(int i = 1 ; i < dataNum;i++){
kikoaac 0:4a1a924f704e 25 RXData[i] = dev.getc();
kikoaac 0:4a1a924f704e 26 }
kikoaac 0:4a1a924f704e 27 //if(DEBUG && !DEBUG_R)sbdbt.printf("L:");
kikoaac 0:4a1a924f704e 28 for(int i = 0 ;i < 4 ; i++){
kikoaac 0:4a1a924f704e 29 floatInByte in;//( (uint16_t)tmp[R][5 + i*2] << 8 ) | (uint16_t)tmp[R][4 + i*2];
kikoaac 0:4a1a924f704e 30 in.c[0] = RXData[4 + i*2];//tmp[R][5 + i*2];
kikoaac 0:4a1a924f704e 31 in.c[1] = RXData[5 + i*2];//tmp[R][4 + i*2];
kikoaac 0:4a1a924f704e 32 uint16_t in_ = ArmSense2[i].read_u16(float(in.si)/0xffff);
kikoaac 0:4a1a924f704e 33 uint16_t intt = map(in_,MinimumRangeL[i],MaxmumRangeL[i],0,65535);
kikoaac 0:4a1a924f704e 34 intt = ReverseL[i] == true ? 0xffff - intt : intt;
kikoaac 0:4a1a924f704e 35 floatInByte intt_;
kikoaac 0:4a1a924f704e 36 intt_.si = intt;
kikoaac 0:4a1a924f704e 37 //if(DEBUG && !DEBUG_R)sbdbt.printf(" %5d ",intt);
kikoaac 0:4a1a924f704e 38 //uint16_t intt = map(in_,13107,52428,0,65535);
kikoaac 0:4a1a924f704e 39 ArmInputSense[L][i].read_u16(float(intt_.si)/0xffff);
kikoaac 0:4a1a924f704e 40 RXData[4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
kikoaac 0:4a1a924f704e 41 RXData[5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕
kikoaac 0:4a1a924f704e 42 }//if(DEBUG && !DEBUG_R)sbdbt.printf("\n");
kikoaac 0:4a1a924f704e 43
kikoaac 0:4a1a924f704e 44 timer.reset();
kikoaac 0:4a1a924f704e 45 send = true;
kikoaac 0:4a1a924f704e 46 }
kikoaac 0:4a1a924f704e 47 }
kikoaac 0:4a1a924f704e 48 void print(int N,char RXdata[12]){
kikoaac 0:4a1a924f704e 49 floatInByte data;
kikoaac 0:4a1a924f704e 50 for(int i = 0 ; i < 2 ; i ++){
kikoaac 0:4a1a924f704e 51 data.c[i] = RXdata[N+i];
kikoaac 0:4a1a924f704e 52 }
kikoaac 0:4a1a924f704e 53 sbdbt.printf("%d ",data.si);
kikoaac 0:4a1a924f704e 54 }
kikoaac 0:4a1a924f704e 55 int id = 0;
kikoaac 0:4a1a924f704e 56 int main() {
kikoaac 0:4a1a924f704e 57
kikoaac 0:4a1a924f704e 58 dev.baud(115200);
kikoaac 0:4a1a924f704e 59
kikoaac 0:4a1a924f704e 60 sbdbt.baud(115200);
kikoaac 0:4a1a924f704e 61 for(int i = 0 ; i < 2; i++)
kikoaac 0:4a1a924f704e 62 {
kikoaac 0:4a1a924f704e 63 tmp[i] = new char[dataNum];
kikoaac 0:4a1a924f704e 64 }
kikoaac 0:4a1a924f704e 65 debugLed1 = true;
kikoaac 0:4a1a924f704e 66 for(int i = 0 ; i < 50 ; i ++ ){
kikoaac 0:4a1a924f704e 67 debugLed2 = !debugLed2;
kikoaac 0:4a1a924f704e 68 wait(0.1);
kikoaac 0:4a1a924f704e 69 }
kikoaac 0:4a1a924f704e 70 dev.attach(RX, Serial::RxIrq);
kikoaac 0:4a1a924f704e 71 dev.putc('L');
kikoaac 0:4a1a924f704e 72 timer.start();
kikoaac 0:4a1a924f704e 73 motionTimer.start();
kikoaac 0:4a1a924f704e 74 while(1) {
kikoaac 0:4a1a924f704e 75 //送信データ格納
kikoaac 0:4a1a924f704e 76
kikoaac 0:4a1a924f704e 77 ctrl.getdata();
kikoaac 0:4a1a924f704e 78 tmp[R][0] = 'H';
kikoaac 0:4a1a924f704e 79 tmp[R][1] = ctrl.analogx();//ヌンチャクアナログX
kikoaac 0:4a1a924f704e 80 tmp[R][2] = ctrl.analogy();//ヌンチャクアナログy
kikoaac 0:4a1a924f704e 81 tmp[R][3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン
kikoaac 0:4a1a924f704e 82 for(int i = 0 ;i < 4 ; i++){
kikoaac 0:4a1a924f704e 83 floatInByte intt_;
kikoaac 0:4a1a924f704e 84 //if(i==0 && DEBUG && DEBUG_R)sbdbt.printf("R:");
kikoaac 0:4a1a924f704e 85 uint16_t in = ArmSense[i].read_u16();
kikoaac 0:4a1a924f704e 86 //if(DEBUG && DEBUG_R)sbdbt.printf(" %5d ",intt);
kikoaac 0:4a1a924f704e 87 uint16_t intt = map(in, MinimumRangeR[i],MaxmumRangeR[i],0,65535);
kikoaac 0:4a1a924f704e 88 intt = ReverseR[i] == true ? 0xffff - intt : intt;
kikoaac 0:4a1a924f704e 89 intt_.si = intt;
kikoaac 0:4a1a924f704e 90 ArmInputSense[R][i].read_u16(float(intt_.si)/0xffff);
kikoaac 0:4a1a924f704e 91 //uint16_t intt = map(in_,13107,52428,0,65535);
kikoaac 0:4a1a924f704e 92 tmp[R][4 + i*2] = intt_.c[0];//uint8_t(intt>>8);//マスター片腕
kikoaac 0:4a1a924f704e 93 tmp[R][5 + i*2] = intt_.c[1];//uint8_t(intt&0xff); //マスター片腕
kikoaac 0:4a1a924f704e 94 }
kikoaac 0:4a1a924f704e 95 tmp[L] = (char*)RXData;
kikoaac 0:4a1a924f704e 96 signed char x1 = map8(tmp[R][1],-90,90,-100,100)+1;
kikoaac 0:4a1a924f704e 97 signed char y1 = map8(tmp[R][2],-90,90,-100,100)-7;
kikoaac 0:4a1a924f704e 98 signed char x2 = map8(tmp[L][1],-90,90,-100,100)+3;
kikoaac 0:4a1a924f704e 99 signed char y2 = map8(tmp[L][2],-90,90,-100,100)-10;
kikoaac 0:4a1a924f704e 100 //sbdbt.printf("%d %d %d %d ",x1,y1,x2,y2);
kikoaac 0:4a1a924f704e 101 sbdbt.printf("sita %d ",impulse(float(y2)/100.0F));
kikoaac 0:4a1a924f704e 102 char** SerialData = ;
kikoaac 0:4a1a924f704e 103 bool txFlag = true;
kikoaac 0:4a1a924f704e 104 switch(int i = MotionSense()){
kikoaac 0:4a1a924f704e 105 case NOTINPUT:
kikoaac 0:4a1a924f704e 106 //tmp[L] = (char*)RXData;
kikoaac 0:4a1a924f704e 107 //SerialData = tmp;
kikoaac 0:4a1a924f704e 108 break;
kikoaac 0:4a1a924f704e 109 case LOADMOTION:
kikoaac 0:4a1a924f704e 110 //sbdbt.printf("LOADMOTION");
kikoaac 0:4a1a924f704e 111 //tmp[L] = (char*)RXData;
kikoaac 0:4a1a924f704e 112 //SerialData = tmp;
kikoaac 0:4a1a924f704e 113 break;
kikoaac 0:4a1a924f704e 114 case TIMEOUT:
kikoaac 0:4a1a924f704e 115 //sbdbt.printf("TIMEOUT");
kikoaac 0:4a1a924f704e 116 //tmp[L] = (char*)RXData;
kikoaac 0:4a1a924f704e 117 //SerialData = tmp;
kikoaac 0:4a1a924f704e 118 break;
kikoaac 0:4a1a924f704e 119 default:
kikoaac 0:4a1a924f704e 120 id = i;
kikoaac 0:4a1a924f704e 121 sbdbt.printf("GET %d ",i);
kikoaac 0:4a1a924f704e 122 txFlag = false;
kikoaac 0:4a1a924f704e 123 break;
kikoaac 0:4a1a924f704e 124 }
kikoaac 0:4a1a924f704e 125 //if(playMotionFlag == true){
kikoaac 0:4a1a924f704e 126 playMotion(0,(uint8_t**)SerialData);
kikoaac 0:4a1a924f704e 127 //}
kikoaac 0:4a1a924f704e 128 //if(DEBUG && DEBUG_R)sbdbt.printf("\n");
kikoaac 0:4a1a924f704e 129 sbdbt.printf("\n");
kikoaac 0:4a1a924f704e 130 //送信データを送る
kikoaac 0:4a1a924f704e 131 //SerialData = tmp;
kikoaac 0:4a1a924f704e 132 if(send == true || txFlag == true){
kikoaac 0:4a1a924f704e 133 for(int j = 0; j < 2 ; j++){
kikoaac 0:4a1a924f704e 134 for(int i = 0 ; i < dataNum ; i++){
kikoaac 0:4a1a924f704e 135 //if(!DEBUG)sbdbt.printf("%3d ",SerialData[j][i]);
kikoaac 0:4a1a924f704e 136 //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
kikoaac 0:4a1a924f704e 137 }
kikoaac 0:4a1a924f704e 138 }
kikoaac 0:4a1a924f704e 139 //sbdbt.printf("R:");
kikoaac 0:4a1a924f704e 140 //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[R]);
kikoaac 0:4a1a924f704e 141 //sbdbt.printf("L:");
kikoaac 0:4a1a924f704e 142 //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[L]);
kikoaac 0:4a1a924f704e 143 for(int j = 0; j < 2 ; j++){
kikoaac 0:4a1a924f704e 144 for(int i = 0 ; i < dataNum ; i++){
kikoaac 0:4a1a924f704e 145 sbdbt.printf("%3d ",SerialData[j][i]);
kikoaac 0:4a1a924f704e 146 //if(!DEBUG)sbdbt.putc(SerialData[j][i]);
kikoaac 0:4a1a924f704e 147 }
kikoaac 0:4a1a924f704e 148 }
kikoaac 0:4a1a924f704e 149 if(!DEBUG)sbdbt.printf("\n");
kikoaac 0:4a1a924f704e 150 }
kikoaac 0:4a1a924f704e 151 dev.putc('L');
kikoaac 0:4a1a924f704e 152 send = false;
kikoaac 0:4a1a924f704e 153 while(timer.read_ms() >= 2000){
kikoaac 0:4a1a924f704e 154 debugLed2 = true;
kikoaac 0:4a1a924f704e 155 waitTime(0.1);
kikoaac 0:4a1a924f704e 156 debugLed2 = false;
kikoaac 0:4a1a924f704e 157 waitTime(0.1);
kikoaac 0:4a1a924f704e 158 }
kikoaac 0:4a1a924f704e 159 debugLed1 = false;
kikoaac 0:4a1a924f704e 160 waitTime(0.01);
kikoaac 0:4a1a924f704e 161 #if DEBUG
kikoaac 0:4a1a924f704e 162 int RL = R;
kikoaac 0:4a1a924f704e 163 //if(DEBUG_R){sbdbt.printf("R:");RL = R;}
kikoaac 0:4a1a924f704e 164 //if(!DEBUG_R){sbdbt.printf("L:");RL = L;}
kikoaac 0:4a1a924f704e 165 //for(int i = 4 ; i < 12 ; i+=2)print(i,SerialData[RL]);
kikoaac 0:4a1a924f704e 166 //sbdbt.printf("\n");
kikoaac 0:4a1a924f704e 167 #endif
kikoaac 0:4a1a924f704e 168 }
kikoaac 0:4a1a924f704e 169 }