Robotics Term Project / Mbed 2 deprecated Robottics_Motion

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:   mbed

ai.cpp@5:794d7d690dc9, 2016-05-24 (annotated)

Committer:
smilestone520
Date:
Tue May 24 07:58:29 2016 +0000
Revision:
5:794d7d690dc9
Parent:
4:ae47210fa346
Child:
6:f4250d71f974
; hahaha

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smilestone520 2:4988d0424f29 1
smilestone520 2:4988d0424f29 2
smilestone520 2:4988d0424f29 3 **** receive and return by bluetooth **************
smilestone520 2:4988d0424f29 4 float xC, yC // car's position
smilestone520 2:4988d0424f29 5
smilestone520 2:4988d0424f29 6 float angleC // car's angle
smilestone520 2:4988d0424f29 7
smilestone520 2:4988d0424f29 8 float xP,yP //position that car need to reach
smilestone520 2:4988d0424f29 9
smilestone520 4:ae47210fa346 10 float angleR // angleR = car-nextSpot direction ---> car direction
smilestone520 2:4988d0424f29 11
smilestone520 2:4988d0424f29 12 **** specs of everything ***************
smilestone520 2:4988d0424f29 13
smilestone520 2:4988d0424f29 14 float longC // car's length
smilestone520 2:4988d0424f29 15
smilestone520 2:4988d0424f29 16 float ballSize // ball size
smilestone520 2:4988d0424f29 17
smilestone520 2:4988d0424f29 18 float longB // long of the court
smilestone520 2:4988d0424f29 19
smilestone520 2:4988d0424f29 20 float wideB // wide of the court
smilestone520 2:4988d0424f29 21
smilestone520 5:794d7d690dc9 22 int mobile_state;
smilestone520 5:794d7d690dc9 23
smilestone520 2:4988d0424f29 24 ***** Code Begin **********
smilestone520 2:4988d0424f29 25
smilestone520 4:ae47210fa346 26 int yB1 yB2 xB3 xB4; // broder conditions
smilestone520 2:4988d0424f29 27
smilestone520 4:ae47210fa346 28 yB1 = longC;
smilestone520 2:4988d0424f29 29
smilestone520 4:ae47210fa346 30 yB2 = wideB - longC ;
smilestone520 2:4988d0424f29 31
smilestone520 4:ae47210fa346 32 xB3 = longC ;
smilestone520 2:4988d0424f29 33
smilestone520 4:ae47210fa346 34 xB4 = longB-longC;
smilestone520 2:4988d0424f29 35 *******************************************************************************
smilestone520 2:4988d0424f29 36
smilestone520 2:4988d0424f29 37 遠離邊界
smilestone520 2:4988d0424f29 38
smilestone520 3:9f5346fc58b3 39 car: away from the border --> turn to the ball direction --> to get the ball --> get ball
smilestone520 3:9f5346fc58b3 40 ---> turn to the point before the gate ---> to the point before the gate --> turn to the gate direction
smilestone520 3:9f5346fc58b3 41 ---> to the gate --> release the ball
smilestone520 2:4988d0424f29 42
smilestone520 2:4988d0424f29 43
smilestone520 2:4988d0424f29 44
smilestone520 2:4988d0424f29 45 matlab:
smilestone520 2:4988d0424f29 46
smilestone520 2:4988d0424f29 47
smilestone520 2:4988d0424f29 48
smilestone520 2:4988d0424f29 49
smilestone520 5:794d7d690dc9 50 ****************** main structure ***********************************
smilestone520 5:794d7d690dc9 51
smilestone520 5:794d7d690dc9 52
smilestone520 5:794d7d690dc9 53 switch(mobile_state)
smilestone520 5:794d7d690dc9 54 {
smilestone520 5:794d7d690dc9 55 case 0:
smilestone520 5:794d7d690dc9 56 // IDLE check if stop color appear
smilestone520 5:794d7d690dc9 57 //check purple color appear or not
smilestone520 5:794d7d690dc9 58
smilestone520 5:794d7d690dc9 59 setSpecs();
smilestone520 5:794d7d690dc9 60 break;
smilestone520 5:794d7d690dc9 61 case 1:
smilestone520 5:794d7d690dc9 62 // set everything icludes the car's and ball's position
smilestone520 5:794d7d690dc9 63 // go to case 2 after get all the information
smilestone520 5:794d7d690dc9 64 break;
smilestone520 5:794d7d690dc9 65 case 2:
smilestone520 5:794d7d690dc9 66 // check if car fit border conditions
smilestone520 5:794d7d690dc9 67 funcBorder();
smilestone520 5:794d7d690dc9 68 to case3
smilestone520 5:794d7d690dc9 69 break;
smilestone520 5:794d7d690dc9 70 case 3://move to get ball
smilestone520 5:794d7d690dc9 71 getBall();
smilestone520 5:794d7d690dc9 72 break;
smilestone520 5:794d7d690dc9 73 case 4: /// move to the gate and release ball
smilestone520 5:794d7d690dc9 74 // move to the point in front of the gate first
smilestone520 5:794d7d690dc9 75 // then head to the gate
smilestone520 5:794d7d690dc9 76 break;
smilestone520 5:794d7d690dc9 77 case 5:
smilestone520 5:794d7d690dc9 78 break;
smilestone520 5:794d7d690dc9 79
smilestone520 5:794d7d690dc9 80 }
smilestone520 5:794d7d690dc9 81
smilestone520 5:794d7d690dc9 82
smilestone520 5:794d7d690dc9 83
smilestone520 5:794d7d690dc9 84
smilestone520 5:794d7d690dc9 85
smilestone520 2:4988d0424f29 86
smilestone520 2:4988d0424f29 87 **************** funcBorder() *****************************************
smilestone520 2:4988d0424f29 88
smilestone520 2:4988d0424f29 89 funcBorder() // check if car is too close to the border
smilestone520 5:794d7d690dc9 90
smilestone520 5:794d7d690dc9 91 // miss the ball restriction part
smilestone520 2:4988d0424f29 92
smilestone520 2:4988d0424f29 93 if(yC > yB1 || yC < yB2 || xC < xB3 || xC > xB4){
smilestone520 2:4988d0424f29 94
smilestone520 2:4988d0424f29 95 if(yC > yB1) // 靠近上邊界
smilestone520 2:4988d0424f29 96 {
smilestone520 2:4988d0424f29 97 if(xC < xB3) //左上角
smilestone520 2:4988d0424f29 98 {
smilestone520 4:ae47210fa346 99 if(angleC <0 && angleC >=-90) // I
smilestone520 2:4988d0424f29 100
smilestone520 4:ae47210fa346 101 //clockwise, to angleC = 45
smilestone520 2:4988d0424f29 102
smilestone520 4:ae47210fa346 103 else if(angleC > -180 && angleC <-90) // II
smilestone520 2:4988d0424f29 104
smilestone520 2:4988d0424f29 105 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 106
smilestone520 4:ae47210fa346 107 else if(angleC <=180 && angleC >=90) // III
smilestone520 2:4988d0424f29 108
smilestone520 4:ae47210fa346 109 //counter clockwise, to angleC = 45
smilestone520 2:4988d0424f29 110
smilestone520 4:ae47210fa346 111 else if(angleC < 90 && angleC >=0) // IV
smilestone520 2:4988d0424f29 112
smilestone520 2:4988d0424f29 113 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 114
smilestone520 2:4988d0424f29 115 }
smilestone520 2:4988d0424f29 116 else if(xC > xB4) //右上角
smilestone520 2:4988d0424f29 117 {
smilestone520 4:ae47210fa346 118 if(angleC <0 && angleC >=-90) // I
smilestone520 2:4988d0424f29 119
smilestone520 2:4988d0424f29 120 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 121
smilestone520 4:ae47210fa346 122 else if(angleC > -180 && angleC <-90) // II
smilestone520 2:4988d0424f29 123
smilestone520 4:ae47210fa346 124 //counter clockwise, to angleC = 135
smilestone520 2:4988d0424f29 125
smilestone520 4:ae47210fa346 126 else if(angleC <=180 && angleC >=90) // III
smilestone520 2:4988d0424f29 127
smilestone520 2:4988d0424f29 128 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 129
smilestone520 4:ae47210fa346 130 else if(angleC < 90 && angleC >=0) // IV
smilestone520 2:4988d0424f29 131
smilestone520 4:ae47210fa346 132 //clockwise, to angleC = 135
smilestone520 2:4988d0424f29 133
smilestone520 2:4988d0424f29 134 }
smilestone520 2:4988d0424f29 135 else //上邊界
smilestone520 2:4988d0424f29 136 {
smilestone520 2:4988d0424f29 137 if(angleC = -90 || angleC = 90)
smilestone520 2:4988d0424f29 138 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 139 else
smilestone520 4:ae47210fa346 140 {smallAngle(90);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 141 }
smilestone520 2:4988d0424f29 142
smilestone520 2:4988d0424f29 143 }
smilestone520 2:4988d0424f29 144 else if(yC < yB2) // 靠近下邊界
smilestone520 2:4988d0424f29 145 {
smilestone520 2:4988d0424f29 146 if(xC < xB3) //左下角
smilestone520 2:4988d0424f29 147 {
smilestone520 4:ae47210fa346 148 if(angleC <0 && angleC >=-90) // I
smilestone520 2:4988d0424f29 149
smilestone520 2:4988d0424f29 150 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 151
smilestone520 4:ae47210fa346 152 else if(angleC > -180 && angleC <-90) // II
smilestone520 2:4988d0424f29 153
smilestone520 4:ae47210fa346 154 //clockwise, to angleC = -45
smilestone520 2:4988d0424f29 155
smilestone520 4:ae47210fa346 156 else if(angleC <=180 && angleC >=90) // III
smilestone520 2:4988d0424f29 157
smilestone520 2:4988d0424f29 158 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 159
smilestone520 4:ae47210fa346 160 else if(angleC < 90 && angleC >=0) // IV
smilestone520 2:4988d0424f29 161
smilestone520 4:ae47210fa346 162 //counter clockwise, to angleC = -45
smilestone520 2:4988d0424f29 163
smilestone520 2:4988d0424f29 164 }
smilestone520 2:4988d0424f29 165 else if(xC > xB4) //右下角
smilestone520 2:4988d0424f29 166 {
smilestone520 4:ae47210fa346 167 if(angleC <0 && angleC >=-90) // I
smilestone520 2:4988d0424f29 168
smilestone520 4:ae47210fa346 169 //counter clockwise, to angleC = -135
smilestone520 2:4988d0424f29 170
smilestone520 4:ae47210fa346 171 else if(angleC > -180 && angleC <-90) // II
smilestone520 2:4988d0424f29 172
smilestone520 2:4988d0424f29 173 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 174
smilestone520 4:ae47210fa346 175 else if(angleC <=180 && angleC >=90) // III
smilestone520 2:4988d0424f29 176
smilestone520 4:ae47210fa346 177 //clockwise, to angleC = -135
smilestone520 2:4988d0424f29 178
smilestone520 4:ae47210fa346 179 else if(angleC < 90 && angleC >=0) // IV
smilestone520 2:4988d0424f29 180
smilestone520 2:4988d0424f29 181 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 182
smilestone520 2:4988d0424f29 183 }
smilestone520 2:4988d0424f29 184 else //下邊界
smilestone520 2:4988d0424f29 185 {
smilestone520 2:4988d0424f29 186 if(angleC = -90 || angleC = 90)
smilestone520 2:4988d0424f29 187 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 188 else
smilestone520 4:ae47210fa346 189 {smallAngle(-90);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 190 }
smilestone520 2:4988d0424f29 191
smilestone520 2:4988d0424f29 192 else if(xC < xB3) //靠近左邊界
smilestone520 2:4988d0424f29 193 {
smilestone520 2:4988d0424f29 194 if(angleC = 0 || angleC = 180)
smilestone520 2:4988d0424f29 195 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 196 else
smilestone520 2:4988d0424f29 197 {smallAngle(0);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 198 }
smilestone520 2:4988d0424f29 199
smilestone520 2:4988d0424f29 200 else if(xC > xB4) //靠近右邊界
smilestone520 2:4988d0424f29 201 {
smilestone520 2:4988d0424f29 202 if(angleC = 0 || angleC = 180)
smilestone520 2:4988d0424f29 203 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 204 else
smilestone520 2:4988d0424f29 205 {smallAngle(180);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 206 }
smilestone520 2:4988d0424f29 207 }
smilestone520 2:4988d0424f29 208
smilestone520 2:4988d0424f29 209 **************** smallAngle() *****************************************
smilestone520 2:4988d0424f29 210
smilestone520 2:4988d0424f29 211 smallAngle( float goodAngle ) // use the smallest turn to right angle ccw / cw, based on 0~360 degree system
smilestone520 2:4988d0424f29 212 {
smilestone520 4:ae47210fa346 213 if(angleR > 0) // CW
smilestone520 2:4988d0424f29 214 {
smilestone520 2:4988d0424f29 215 // turn clockwise to goodAngle;
smilestone520 2:4988d0424f29 216 }
smilestone520 4:ae47210fa346 217 else if(angleR < 0) //CCW
smilestone520 2:4988d0424f29 218 {
smilestone520 2:4988d0424f29 219 // turn counter clockwise to goodAngle;
smilestone520 2:4988d0424f29 220 }
smilestone520 2:4988d0424f29 221 }
smilestone520 2:4988d0424f29 222
smilestone520 2:4988d0424f29 223 *************** turnCW() ***********************************
smilestone520 2:4988d0424f29 224
smilestone520 2:4988d0424f29 225 turnCW(float goodAngle)
smilestone520 2:4988d0424f29 226 {
smilestone520 2:4988d0424f29 227 // turn clockwise to goodAngle;
smilestone520 2:4988d0424f29 228 }
smilestone520 2:4988d0424f29 229
smilestone520 2:4988d0424f29 230 *************** turnCCW() ***********************************
smilestone520 2:4988d0424f29 231
smilestone520 2:4988d0424f29 232 turnCCW(float goodAngle)
smilestone520 2:4988d0424f29 233 {
smilestone520 2:4988d0424f29 234 // turn counter clockwise to goodAngle;
smilestone520 2:4988d0424f29 235 }
smilestone520 2:4988d0424f29 236
smilestone520 2:4988d0424f29 237
smilestone520 2:4988d0424f29 238 **********
smilestone520 2:4988d0424f29 239
smilestone520 2:4988d0424f29 240
smilestone520 2:4988d0424f29 241
smilestone520 2:4988d0424f29 242
smilestone520 2:4988d0424f29 243
smilestone520 2:4988d0424f29 244
smilestone520 2:4988d0424f29 245
smilestone520 2:4988d0424f29 246
smilestone520 2:4988d0424f29 247
smilestone520 2:4988d0424f29 248
smilestone520 2:4988d0424f29 249
smilestone520 2:4988d0424f29 250
smilestone520 2:4988d0424f29 251
smilestone520 2:4988d0424f29 252
smilestone520 2:4988d0424f29 253
smilestone520 2:4988d0424f29 254
smilestone520 2:4988d0424f29 255
smilestone520 2:4988d0424f29 256
smilestone520 2:4988d0424f29 257
smilestone520 2:4988d0424f29 258
smilestone520 2:4988d0424f29 259
smilestone520 2:4988d0424f29 260
smilestone520 2:4988d0424f29 261
smilestone520 2:4988d0424f29 262
smilestone520 2:4988d0424f29 263