takaaki mastuzawa / Mbed 2 deprecated SAEKI_SUMOU_2015

mbed_pro_miniを使った相撲ロボットプログラムの雛形

Dependencies:   RemoteIR USBDevice mbed

main.cpp@0:caf66b280785, 2015-09-10 (annotated)

Committer:
hardtail
Date:
Thu Sep 10 09:33:58 2015 +0000
Revision:
0:caf66b280785
mbed_pro_mini????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hardtail 0:caf66b280785 1 #include "mbed.h"
hardtail 0:caf66b280785 2 #include "USBSerial.h"
hardtail 0:caf66b280785 3 #include "ReceiverIR.h"
hardtail 0:caf66b280785 4
hardtail 0:caf66b280785 5 #define LED1 P0_29
hardtail 0:caf66b280785 6 #define LED2 P0_28
hardtail 0:caf66b280785 7 #define LED3 P0_27
hardtail 0:caf66b280785 8 #define LED4 P0_26
hardtail 0:caf66b280785 9
hardtail 0:caf66b280785 10 #define front 1
hardtail 0:caf66b280785 11 #define back 2
hardtail 0:caf66b280785 12 #define left 3
hardtail 0:caf66b280785 13 #define right 4
hardtail 0:caf66b280785 14 #define breaking 5
hardtail 0:caf66b280785 15
hardtail 0:caf66b280785 16 #define IRrecerver P0_5
hardtail 0:caf66b280785 17
hardtail 0:caf66b280785 18 /*******X-box rimote controller********/
hardtail 0:caf66b280785 19 #define Xbutton 1
hardtail 0:caf66b280785 20 #define powoer 2
hardtail 0:caf66b280785 21 #define startBTN 3
hardtail 0:caf66b280785 22 #define deside 4
hardtail 0:caf66b280785 23 #define windows 5
hardtail 0:caf66b280785 24 /**************************************/
hardtail 0:caf66b280785 25
hardtail 0:caf66b280785 26 //USBSerial pc;
hardtail 0:caf66b280785 27
hardtail 0:caf66b280785 28 DigitalOut led[] =
hardtail 0:caf66b280785 29 {DigitalOut(LED1),
hardtail 0:caf66b280785 30 DigitalOut(LED2),
hardtail 0:caf66b280785 31 DigitalOut(LED3),
hardtail 0:caf66b280785 32 DigitalOut(LED4)};
hardtail 0:caf66b280785 33
hardtail 0:caf66b280785 34 DigitalOut myled(P0_25);
hardtail 0:caf66b280785 35
hardtail 0:caf66b280785 36 DigitalOut motorR[] = {DigitalOut(P0_0), DigitalOut(P0_1)};
hardtail 0:caf66b280785 37
hardtail 0:caf66b280785 38 DigitalOut motorL[] = {DigitalOut(P0_2), DigitalOut(P0_3)};
hardtail 0:caf66b280785 39
hardtail 0:caf66b280785 40 DigitalIn sensor[] = {
hardtail 0:caf66b280785 41 DigitalIn(P0_10),
hardtail 0:caf66b280785 42 DigitalIn(P0_11),
hardtail 0:caf66b280785 43 DigitalIn(P0_12),
hardtail 0:caf66b280785 44 DigitalIn(P0_13),
hardtail 0:caf66b280785 45 DigitalIn(P0_15)};
hardtail 0:caf66b280785 46
hardtail 0:caf66b280785 47 DigitalIn dip[] = {
hardtail 0:caf66b280785 48 DigitalIn(P0_9),
hardtail 0:caf66b280785 49 DigitalIn(P0_8),
hardtail 0:caf66b280785 50 DigitalIn(P0_7),
hardtail 0:caf66b280785 51 DigitalIn(P0_6)};
hardtail 0:caf66b280785 52
hardtail 0:caf66b280785 53 DigitalIn LineSensor[] = {DigitalIn(P0_17),DigitalIn(P0_15),DigitalIn(P0_20),DigitalIn(P0_23)};
hardtail 0:caf66b280785 54
hardtail 0:caf66b280785 55 ReceiverIR ir_rx(IRrecerver);
hardtail 0:caf66b280785 56 RemoteIR::Format format;
hardtail 0:caf66b280785 57
hardtail 0:caf66b280785 58 int IRswitch();
hardtail 0:caf66b280785 59 int IRstop();
hardtail 0:caf66b280785 60 void IRclear();
hardtail 0:caf66b280785 61
hardtail 0:caf66b280785 62 Ticker motorTest;
hardtail 0:caf66b280785 63 int mTestStatus = 0;
hardtail 0:caf66b280785 64
hardtail 0:caf66b280785 65 Timer modeCnt;
hardtail 0:caf66b280785 66 Timer startCnt;
hardtail 0:caf66b280785 67 Timer rollingCnt;
hardtail 0:caf66b280785 68
hardtail 0:caf66b280785 69 void go (int);
hardtail 0:caf66b280785 70 void flip();
hardtail 0:caf66b280785 71 void EnemySensWait(int);
hardtail 0:caf66b280785 72
hardtail 0:caf66b280785 73 int nowMode = 1;
hardtail 0:caf66b280785 74 int waitFlag = 0;
hardtail 0:caf66b280785 75 int fightingMode = 0;
hardtail 0:caf66b280785 76 int enemySensorEnble = 0;
hardtail 0:caf66b280785 77 int startFlag = 0;
hardtail 0:caf66b280785 78 int goFlag = 0;
hardtail 0:caf66b280785 79 int startSelect = 3;
hardtail 0:caf66b280785 80 int enemySensActive = 1;
hardtail 0:caf66b280785 81
hardtail 0:caf66b280785 82 int main() {
hardtail 0:caf66b280785 83 //motorTest.attach(&flip, 1.0);
hardtail 0:caf66b280785 84
hardtail 0:caf66b280785 85 for(int i=0; i< 5; i++) sensor[i].mode(PullUp);
hardtail 0:caf66b280785 86 for(int i=0; i< 4; i++) dip[i].mode(PullDown);
hardtail 0:caf66b280785 87
hardtail 0:caf66b280785 88 for(int i=0;i<4; i++)LineSensor[i].mode(PullUp);
hardtail 0:caf66b280785 89 LineSensor[2].mode(PullUp);
hardtail 0:caf66b280785 90
hardtail 0:caf66b280785 91 int stop = 1;
hardtail 0:caf66b280785 92
hardtail 0:caf66b280785 93 while(stop){
hardtail 0:caf66b280785 94 if(IRswitch() == Xbutton) stop = 0;
hardtail 0:caf66b280785 95 wait(0.05);
hardtail 0:caf66b280785 96 }
hardtail 0:caf66b280785 97
hardtail 0:caf66b280785 98 wait(5);
hardtail 0:caf66b280785 99
hardtail 0:caf66b280785 100 int start = 1;
hardtail 0:caf66b280785 101
hardtail 0:caf66b280785 102
hardtail 0:caf66b280785 103 while(start){
hardtail 0:caf66b280785 104
hardtail 0:caf66b280785 105 go(front);
hardtail 0:caf66b280785 106
hardtail 0:caf66b280785 107 //if(LineSensor[] == )
hardtail 0:caf66b280785 108
hardtail 0:caf66b280785 109 if(IRswitch() == powoer) start = 0;
hardtail 0:caf66b280785 110 }
hardtail 0:caf66b280785 111
hardtail 0:caf66b280785 112 while(1) {
hardtail 0:caf66b280785 113 //for(int i=0; i< 4; i++) led[i] = sensor[i];
hardtail 0:caf66b280785 114 for(int i=0; i< 4; i++) led[i] = LineSensor[i];
hardtail 0:caf66b280785 115 myled = LineSensor[0];
hardtail 0:caf66b280785 116 /*
hardtail 0:caf66b280785 117 for(int i=0; i< 4; i++) led[i] = dip[i];
hardtail 0:caf66b280785 118 myled = sensor[2];
hardtail 0:caf66b280785 119
hardtail 0:caf66b280785 120 if(dip[1]) go(front);
hardtail 0:caf66b280785 121 if(dip[2]) go(left);
hardtail 0:caf66b280785 122 if(dip[3]) go(right);
hardtail 0:caf66b280785 123 if(dip[1]==0 && dip[2]==0 && dip[3]==0) go(breaking);
hardtail 0:caf66b280785 124 */
hardtail 0:caf66b280785 125 wait(0.05);
hardtail 0:caf66b280785 126
hardtail 0:caf66b280785 127 }
hardtail 0:caf66b280785 128 }
hardtail 0:caf66b280785 129
hardtail 0:caf66b280785 130 int IRswitch(){
hardtail 0:caf66b280785 131 uint8_t buf[32];
hardtail 0:caf66b280785 132 int bitcount;
hardtail 0:caf66b280785 133 int res = 0;
hardtail 0:caf66b280785 134 int i;
hardtail 0:caf66b280785 135
hardtail 0:caf66b280785 136 res = 0;
hardtail 0:caf66b280785 137
hardtail 0:caf66b280785 138 if (ir_rx.getState() == ReceiverIR::Received) {
hardtail 0:caf66b280785 139 bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
hardtail 0:caf66b280785 140
hardtail 0:caf66b280785 141 //pc.printf("reserve : ");
hardtail 0:caf66b280785 142
hardtail 0:caf66b280785 143 if((bitcount > 30)){
hardtail 0:caf66b280785 144 #if 0
hardtail 0:caf66b280785 145 for(i=0;i<sizeof(buf);i++){
hardtail 0:caf66b280785 146 pc.printf("%x,",buf[i]);
hardtail 0:caf66b280785 147 }
hardtail 0:caf66b280785 148 #endif
hardtail 0:caf66b280785 149 //if(buf[1]==0x80) pc.printf("OK:");
hardtail 0:caf66b280785 150 if(buf[3]==0x24||buf[3]==0x12) {
hardtail 0:caf66b280785 151 //pc.printf("X\n");
hardtail 0:caf66b280785 152 res = Xbutton;
hardtail 0:caf66b280785 153 }
hardtail 0:caf66b280785 154 if(buf[3]==0x20||buf[3]==0x10) {
hardtail 0:caf66b280785 155 //pc.printf("powoer\n");
hardtail 0:caf66b280785 156 res = powoer;
hardtail 0:caf66b280785 157 }
hardtail 0:caf66b280785 158 if(buf[3]==0x30||buf[3]==0x18) {
hardtail 0:caf66b280785 159 //pc.printf("start\n");
hardtail 0:caf66b280785 160 res = startBTN;
hardtail 0:caf66b280785 161 }
hardtail 0:caf66b280785 162 if(buf[3]==0x50||buf[3]==0xa0) {
hardtail 0:caf66b280785 163 //pc.printf("win\n");
hardtail 0:caf66b280785 164 res = windows;
hardtail 0:caf66b280785 165 }
hardtail 0:caf66b280785 166 //pc.printf("\n");
hardtail 0:caf66b280785 167 }
hardtail 0:caf66b280785 168 }
hardtail 0:caf66b280785 169
hardtail 0:caf66b280785 170 return res;
hardtail 0:caf66b280785 171 }
hardtail 0:caf66b280785 172
hardtail 0:caf66b280785 173 void IRclear(){
hardtail 0:caf66b280785 174 uint8_t buf[32];
hardtail 0:caf66b280785 175
hardtail 0:caf66b280785 176 while (ir_rx.getState() == ReceiverIR::Received){
hardtail 0:caf66b280785 177 ir_rx.getData(&format, buf, sizeof(buf) * 8);
hardtail 0:caf66b280785 178 }
hardtail 0:caf66b280785 179 }
hardtail 0:caf66b280785 180
hardtail 0:caf66b280785 181 void go (int mode){
hardtail 0:caf66b280785 182 switch(mode){
hardtail 0:caf66b280785 183 case front:
hardtail 0:caf66b280785 184 motorR[0] = 0;
hardtail 0:caf66b280785 185 motorR[1] = 1;
hardtail 0:caf66b280785 186
hardtail 0:caf66b280785 187 motorL[0] = 0;
hardtail 0:caf66b280785 188 motorL[1] = 1;
hardtail 0:caf66b280785 189
hardtail 0:caf66b280785 190
hardtail 0:caf66b280785 191 break;
hardtail 0:caf66b280785 192
hardtail 0:caf66b280785 193 case back:
hardtail 0:caf66b280785 194 motorR[0] = 1;
hardtail 0:caf66b280785 195 motorR[1] = 0;
hardtail 0:caf66b280785 196
hardtail 0:caf66b280785 197 motorL[0] = 1;
hardtail 0:caf66b280785 198 motorL[1] = 0;
hardtail 0:caf66b280785 199 break;
hardtail 0:caf66b280785 200
hardtail 0:caf66b280785 201 case left:
hardtail 0:caf66b280785 202 motorR[0] = 0;
hardtail 0:caf66b280785 203 motorR[1] = 1;
hardtail 0:caf66b280785 204
hardtail 0:caf66b280785 205 motorL[0] = 1;
hardtail 0:caf66b280785 206 motorL[1] = 0;
hardtail 0:caf66b280785 207 break;
hardtail 0:caf66b280785 208
hardtail 0:caf66b280785 209 case right:
hardtail 0:caf66b280785 210 motorR[0] = 1;
hardtail 0:caf66b280785 211 motorR[1] = 0;
hardtail 0:caf66b280785 212
hardtail 0:caf66b280785 213 motorL[0] = 0;
hardtail 0:caf66b280785 214 motorL[1] = 1;
hardtail 0:caf66b280785 215 break;
hardtail 0:caf66b280785 216
hardtail 0:caf66b280785 217 case breaking:
hardtail 0:caf66b280785 218 motorR[0] = 1;
hardtail 0:caf66b280785 219 motorR[1] = 1;
hardtail 0:caf66b280785 220
hardtail 0:caf66b280785 221 motorL[0] = 1;
hardtail 0:caf66b280785 222 motorL[1] = 1;
hardtail 0:caf66b280785 223 break;
hardtail 0:caf66b280785 224 }
hardtail 0:caf66b280785 225 }
hardtail 0:caf66b280785 226
hardtail 0:caf66b280785 227 void flip(){
hardtail 0:caf66b280785 228 switch(mTestStatus){
hardtail 0:caf66b280785 229 case 0:
hardtail 0:caf66b280785 230 motorR[0] = 1;
hardtail 0:caf66b280785 231 motorR[1] = 1;
hardtail 0:caf66b280785 232
hardtail 0:caf66b280785 233 motorL[0] = 1;
hardtail 0:caf66b280785 234 motorL[1] = 1;
hardtail 0:caf66b280785 235 mTestStatus = 1;
hardtail 0:caf66b280785 236 break;
hardtail 0:caf66b280785 237 case 1:
hardtail 0:caf66b280785 238 motorR[0] = 1;
hardtail 0:caf66b280785 239 motorR[1] = 0;
hardtail 0:caf66b280785 240
hardtail 0:caf66b280785 241 motorL[0] = 0;
hardtail 0:caf66b280785 242 motorL[1] = 1;
hardtail 0:caf66b280785 243 mTestStatus = 2;
hardtail 0:caf66b280785 244 break;
hardtail 0:caf66b280785 245 case 2:
hardtail 0:caf66b280785 246 motorR[0] = 0;
hardtail 0:caf66b280785 247 motorR[1] = 1;
hardtail 0:caf66b280785 248
hardtail 0:caf66b280785 249 motorL[0] = 1;
hardtail 0:caf66b280785 250 motorL[1] = 0;
hardtail 0:caf66b280785 251 mTestStatus = 3;
hardtail 0:caf66b280785 252 break;
hardtail 0:caf66b280785 253 case 3:
hardtail 0:caf66b280785 254 motorR[0] = 0;
hardtail 0:caf66b280785 255 motorR[1] = 0;
hardtail 0:caf66b280785 256
hardtail 0:caf66b280785 257 motorL[0] = 0;
hardtail 0:caf66b280785 258 motorL[1] = 0;
hardtail 0:caf66b280785 259 mTestStatus = 0;
hardtail 0:caf66b280785 260 break;
hardtail 0:caf66b280785 261 }
hardtail 0:caf66b280785 262 }
hardtail 0:caf66b280785 263
hardtail 0:caf66b280785 264 void EnemySensWait(int rollTime){
hardtail 0:caf66b280785 265 rollingCnt.reset();
hardtail 0:caf66b280785 266 rollingCnt.start();
hardtail 0:caf66b280785 267 while(sensor[2] == 1 && rollingCnt.read_ms() < rollTime);
hardtail 0:caf66b280785 268 rollingCnt.stop();
hardtail 0:caf66b280785 269 }