CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T

CatPot for defence on RoboCup in 2015 winter

Dependencies:   AQM0802A HMC6352 MultiSerial PID Servo mbed

Loop.txt@4:7c488c059498, 2015-03-13 (annotated)

Committer:
lilac0112_1
Date:
Fri Mar 13 02:10:37 2015 +0000
Revision:
4:7c488c059498
Child:
6:2c9cb40ada59
haven't written completely.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 4:7c488c059498 1 while(0){
lilac0112_1 4:7c488c059498 2
lilac0112_1 4:7c488c059498 3 //S555.calibrate(0.0006, 120.0);
lilac0112_1 4:7c488c059498 4
lilac0112_1 4:7c488c059498 5 //Receive();
lilac0112_1 4:7c488c059498 6 //Lcd.printf("%d\n",IrNum);
lilac0112_1 4:7c488c059498 7 /*白線を読んでいないか確認する*/
lilac0112_1 4:7c488c059498 8 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 4:7c488c059498 9
lilac0112_1 4:7c488c059498 10 /*
lilac0112_1 4:7c488c059498 11 if(LineData){
lilac0112_1 4:7c488c059498 12 LineIr = LineData & IrChange[IrNum];
lilac0112_1 4:7c488c059498 13 LinePing = PingChange(LineData);
lilac0112_1 4:7c488c059498 14 if(LineIr){
lilac0112_1 4:7c488c059498 15 move(0,0,0,LastDegree);
lilac0112_1 4:7c488c059498 16 while(LineIr){
lilac0112_1 4:7c488c059498 17 Led[1] = Led[2] = Led[3] = 1;
lilac0112_1 4:7c488c059498 18 //Receive();
lilac0112_1 4:7c488c059498 19 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 4:7c488c059498 20 LineIr = LineData & IrChange[IrNum];
lilac0112_1 4:7c488c059498 21 wait_ms(10);
lilac0112_1 4:7c488c059498 22 }
lilac0112_1 4:7c488c059498 23 }else if(LinePing){
lilac0112_1 4:7c488c059498 24 move(0,0,0,LastDegree);
lilac0112_1 4:7c488c059498 25 while(LinePing){
lilac0112_1 4:7c488c059498 26 Led[1] = Led[2] = Led[3] = 1;
lilac0112_1 4:7c488c059498 27 //Receive();
lilac0112_1 4:7c488c059498 28 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 4:7c488c059498 29 LinePing = PingChange(LineData);
lilac0112_1 4:7c488c059498 30
lilac0112_1 4:7c488c059498 31 wait_ms(10);
lilac0112_1 4:7c488c059498 32 }
lilac0112_1 4:7c488c059498 33 }
lilac0112_1 4:7c488c059498 34
lilac0112_1 4:7c488c059498 35
lilac0112_1 4:7c488c059498 36 Led[1] = Led[2] = Led[3] = 0;
lilac0112_1 4:7c488c059498 37
lilac0112_1 4:7c488c059498 38 }
lilac0112_1 4:7c488c059498 39 */
lilac0112_1 4:7c488c059498 40
lilac0112_1 4:7c488c059498 41 /*
lilac0112_1 4:7c488c059498 42 Power = 0;
lilac0112_1 4:7c488c059498 43 Led[0] = 1;
lilac0112_1 4:7c488c059498 44 Degree = 0;
lilac0112_1 4:7c488c059498 45 SetC = 0.0;
lilac0112_1 4:7c488c059498 46 */
lilac0112_1 4:7c488c059498 47
lilac0112_1 4:7c488c059498 48 Led[3] = 1;
lilac0112_1 4:7c488c059498 49 //Receive();
lilac0112_1 4:7c488c059498 50 //Degree = IrDegree();
lilac0112_1 4:7c488c059498 51
lilac0112_1 4:7c488c059498 52 /*if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
lilac0112_1 4:7c488c059498 53 (AnotherAction[IrNum/6])();
lilac0112_1 4:7c488c059498 54 continue;
lilac0112_1 4:7c488c059498 55 }*/
lilac0112_1 4:7c488c059498 56
lilac0112_1 4:7c488c059498 57 if(IrNum == 12){
lilac0112_1 4:7c488c059498 58
lilac0112_1 4:7c488c059498 59 (AnotherAction[2])();//GoHome();
lilac0112_1 4:7c488c059498 60 //move(0,0,0,LastDegree);
lilac0112_1 4:7c488c059498 61 wait_ms(10);
lilac0112_1 4:7c488c059498 62 //continue;
lilac0112_1 4:7c488c059498 63 }
lilac0112_1 4:7c488c059498 64
lilac0112_1 4:7c488c059498 65 /*
lilac0112_1 4:7c488c059498 66 nDegree = wrapDegree[Degree/15];
lilac0112_1 4:7c488c059498 67 Power = 20;
lilac0112_1 4:7c488c059498 68 */
lilac0112_1 4:7c488c059498 69
lilac0112_1 4:7c488c059498 70 // Degree = nDegree + addDegree;
lilac0112_1 4:7c488c059498 71
lilac0112_1 4:7c488c059498 72
lilac0112_1 4:7c488c059498 73 if((Degree <0)||(Degree>=360)){
lilac0112_1 4:7c488c059498 74 Degree = 0;
lilac0112_1 4:7c488c059498 75 }
lilac0112_1 4:7c488c059498 76 //fool(&Degree,&Power);
lilac0112_1 4:7c488c059498 77 move(Power,Power,CompassPID,Degree);
lilac0112_1 4:7c488c059498 78
lilac0112_1 4:7c488c059498 79 //wait_ms(500);
lilac0112_1 4:7c488c059498 80 Led[0] =0;
lilac0112_1 4:7c488c059498 81 wait_ms(10);
lilac0112_1 4:7c488c059498 82
lilac0112_1 4:7c488c059498 83 }
lilac0112_1 4:7c488c059498 84
lilac0112_1 4:7c488c059498 85 while(0){
lilac0112_1 4:7c488c059498 86 //デモプログラム
lilac0112_1 4:7c488c059498 87 //Receive();
lilac0112_1 4:7c488c059498 88 //pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
lilac0112_1 4:7c488c059498 89 pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
lilac0112_1 4:7c488c059498 90
lilac0112_1 4:7c488c059498 91 //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]);
lilac0112_1 4:7c488c059498 92 //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
lilac0112_1 4:7c488c059498 93
lilac0112_1 4:7c488c059498 94 /*for(int i=0; i<13; i++){
lilac0112_1 4:7c488c059498 95 move(vrl[i],vrl[i],0,IrServo[i]);
lilac0112_1 4:7c488c059498 96 wait(0.5);
lilac0112_1 4:7c488c059498 97 }*/
lilac0112_1 4:7c488c059498 98
lilac0112_1 4:7c488c059498 99 /*if(PingData[2]>=40){
lilac0112_1 4:7c488c059498 100 move(vrl[6],vrl[6],0,IrServo[6]);
lilac0112_1 4:7c488c059498 101 }*/
lilac0112_1 4:7c488c059498 102
lilac0112_1 4:7c488c059498 103 /*
lilac0112_1 4:7c488c059498 104 if(
lilac0112_1 4:7c488c059498 105 (
lilac0112_1 4:7c488c059498 106 (
lilac0112_1 4:7c488c059498 107 (
lilac0112_1 4:7c488c059498 108 (Compass / 10) + 540 - CompassDef
lilac0112_1 4:7c488c059498 109 ) % 360
lilac0112_1 4:7c488c059498 110 ) - REFERENCE
lilac0112_1 4:7c488c059498 111 ) >15.0
lilac0112_1 4:7c488c059498 112 ){
lilac0112_1 4:7c488c059498 113
lilac0112_1 4:7c488c059498 114 move(0,0,0,0);
lilac0112_1 4:7c488c059498 115 continue;
lilac0112_1 4:7c488c059498 116 }
lilac0112_1 4:7c488c059498 117
lilac0112_1 4:7c488c059498 118 if(PingData[2]>=40){
lilac0112_1 4:7c488c059498 119 move(vrl[6],vrl[6],0,IrServo[6]);
lilac0112_1 4:7c488c059498 120 continue;
lilac0112_1 4:7c488c059498 121 }
lilac0112_1 4:7c488c059498 122
lilac0112_1 4:7c488c059498 123 GoHome();*/
lilac0112_1 4:7c488c059498 124
lilac0112_1 4:7c488c059498 125 }
lilac0112_1 4:7c488c059498 126
lilac0112_1 4:7c488c059498 127 while(1){
lilac0112_1 4:7c488c059498 128 //デモプログラム
lilac0112_1 4:7c488c059498 129
lilac0112_1 4:7c488c059498 130
lilac0112_1 4:7c488c059498 131 //ping continue;
lilac0112_1 4:7c488c059498 132 //line continue;
lilac0112_1 4:7c488c059498 133 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 4:7c488c059498 134
lilac0112_1 4:7c488c059498 135 if(Line!=0){
lilac0112_1 4:7c488c059498 136 if(LastPwm[0]==0){
lilac0112_1 4:7c488c059498 137
lilac0112_1 4:7c488c059498 138 }
lilac0112_1 4:7c488c059498 139 move(LastPwm[0]*(-1),LastPwm[3]*(-1)*(-1),0,LastDeg);
lilac0112_1 4:7c488c059498 140 wait(0.5);
lilac0112_1 4:7c488c059498 141 continue;
lilac0112_1 4:7c488c059498 142 }
lilac0112_1 4:7c488c059498 143
lilac0112_1 4:7c488c059498 144 /*
lilac0112_1 4:7c488c059498 145 if(((((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE) >15.0){
lilac0112_1 4:7c488c059498 146
lilac0112_1 4:7c488c059498 147 move(0,0,0,0);
lilac0112_1 4:7c488c059498 148 continue;
lilac0112_1 4:7c488c059498 149 }
lilac0112_1 4:7c488c059498 150
lilac0112_1 4:7c488c059498 151 if(PingData[2]>=40){
lilac0112_1 4:7c488c059498 152 move(vrl[6],vrl[6],0,IrServo[6]);
lilac0112_1 4:7c488c059498 153 continue;
lilac0112_1 4:7c488c059498 154 }
lilac0112_1 4:7c488c059498 155 */
lilac0112_1 4:7c488c059498 156 /*if(PingData[2]>=40){
lilac0112_1 4:7c488c059498 157
lilac0112_1 4:7c488c059498 158 move(vrl[6],vrl[6],0,IrServo[6]);
lilac0112_1 4:7c488c059498 159 continue;
lilac0112_1 4:7c488c059498 160 }*/
lilac0112_1 4:7c488c059498 161
lilac0112_1 4:7c488c059498 162 Gap=2;
lilac0112_1 4:7c488c059498 163
lilac0112_1 4:7c488c059498 164 switch(IrNum){
lilac0112_1 4:7c488c059498 165 case 0:
lilac0112_1 4:7c488c059498 166 case 1:
lilac0112_1 4:7c488c059498 167 case 2:
lilac0112_1 4:7c488c059498 168 case 3:
lilac0112_1 4:7c488c059498 169 moving(IrNum);
lilac0112_1 4:7c488c059498 170 break;
lilac0112_1 4:7c488c059498 171 case 4:
lilac0112_1 4:7c488c059498 172 case 5:
lilac0112_1 4:7c488c059498 173 moving(IrNum+Gap);
lilac0112_1 4:7c488c059498 174 break;
lilac0112_1 4:7c488c059498 175 case 6:
lilac0112_1 4:7c488c059498 176 if(PingData[0]>PingData[1]) moving(IrNum+Gap);
lilac0112_1 4:7c488c059498 177 else moving(IrNum-Gap);
lilac0112_1 4:7c488c059498 178
lilac0112_1 4:7c488c059498 179 break;
lilac0112_1 4:7c488c059498 180 case 7:
lilac0112_1 4:7c488c059498 181 case 8:
lilac0112_1 4:7c488c059498 182 moving(IrNum-Gap);
lilac0112_1 4:7c488c059498 183 break;
lilac0112_1 4:7c488c059498 184 case 9:
lilac0112_1 4:7c488c059498 185 case 10:
lilac0112_1 4:7c488c059498 186 case 11:
lilac0112_1 4:7c488c059498 187 moving(IrNum);
lilac0112_1 4:7c488c059498 188 break;
lilac0112_1 4:7c488c059498 189 case 12:
lilac0112_1 4:7c488c059498 190 GoHome();
lilac0112_1 4:7c488c059498 191 break;
lilac0112_1 4:7c488c059498 192
lilac0112_1 4:7c488c059498 193 }
lilac0112_1 4:7c488c059498 194
lilac0112_1 4:7c488c059498 195
lilac0112_1 4:7c488c059498 196 }
lilac0112_1 4:7c488c059498 197
lilac0112_1 4:7c488c059498 198 while(0){
lilac0112_1 4:7c488c059498 199 moving(6);
lilac0112_1 4:7c488c059498 200 }