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@12:c1a667ca6c53, 2016-05-25 (annotated)

Committer:
smilestone520
Date:
Wed May 25 10:39:33 2016 +0000
Revision:
12:c1a667ca6c53
Parent:
8:079c3326816e 
real

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smilestone520 2:4988d0424f29 1
smilestone520 2:4988d0424f29 2
roger5641 8:079c3326816e 3 **** receive and return by bluetooth ************** // bluetooth.getc()
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 6:f4250d71f974 55 case 0: // IDLE
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 6:f4250d71f974 61 case 1: // get first information
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 6:f4250d71f974 65 case 2: /// border condition
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 6:f4250d71f974 71
smilestone520 5:794d7d690dc9 72 getBall();
smilestone520 5:794d7d690dc9 73 break;
smilestone520 5:794d7d690dc9 74 case 4: /// move to the gate and release ball
smilestone520 5:794d7d690dc9 75 // move to the point in front of the gate first
smilestone520 5:794d7d690dc9 76 // then head to the gate
smilestone520 5:794d7d690dc9 77 break;
smilestone520 5:794d7d690dc9 78 case 5:
smilestone520 5:794d7d690dc9 79 break;
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 5:794d7d690dc9 86
smilestone520 2:4988d0424f29 87
smilestone520 2:4988d0424f29 88 **************** funcBorder() *****************************************
smilestone520 2:4988d0424f29 89
smilestone520 2:4988d0424f29 90 funcBorder() // check if car is too close to the border
smilestone520 5:794d7d690dc9 91
smilestone520 5:794d7d690dc9 92 // miss the ball restriction part
smilestone520 2:4988d0424f29 93
smilestone520 2:4988d0424f29 94 if(yC > yB1 || yC < yB2 || xC < xB3 || xC > xB4){
smilestone520 2:4988d0424f29 95
smilestone520 2:4988d0424f29 96 if(yC > yB1) // 靠近上邊界
smilestone520 2:4988d0424f29 97 {
smilestone520 2:4988d0424f29 98 if(xC < xB3) //左上角
smilestone520 2:4988d0424f29 99 {
smilestone520 4:ae47210fa346 100 if(angleC <0 && angleC >=-90) // I
smilestone520 2:4988d0424f29 101
smilestone520 4:ae47210fa346 102 //clockwise, to angleC = 45
smilestone520 2:4988d0424f29 103
smilestone520 4:ae47210fa346 104 else if(angleC > -180 && angleC <-90) // II
smilestone520 2:4988d0424f29 105
smilestone520 2:4988d0424f29 106 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 107
smilestone520 4:ae47210fa346 108 else if(angleC <=180 && angleC >=90) // III
smilestone520 2:4988d0424f29 109
smilestone520 4:ae47210fa346 110 //counter clockwise, to angleC = 45
smilestone520 2:4988d0424f29 111
smilestone520 4:ae47210fa346 112 else if(angleC < 90 && angleC >=0) // IV
smilestone520 2:4988d0424f29 113
smilestone520 2:4988d0424f29 114 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 115
smilestone520 2:4988d0424f29 116 }
smilestone520 2:4988d0424f29 117 else if(xC > xB4) //右上角
smilestone520 2:4988d0424f29 118 {
smilestone520 4:ae47210fa346 119 if(angleC <0 && angleC >=-90) // I
smilestone520 2:4988d0424f29 120
smilestone520 2:4988d0424f29 121 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 122
smilestone520 4:ae47210fa346 123 else if(angleC > -180 && angleC <-90) // II
smilestone520 2:4988d0424f29 124
smilestone520 4:ae47210fa346 125 //counter clockwise, to angleC = 135
smilestone520 2:4988d0424f29 126
smilestone520 4:ae47210fa346 127 else if(angleC <=180 && angleC >=90) // III
smilestone520 2:4988d0424f29 128
smilestone520 2:4988d0424f29 129 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 130
smilestone520 4:ae47210fa346 131 else if(angleC < 90 && angleC >=0) // IV
smilestone520 2:4988d0424f29 132
smilestone520 4:ae47210fa346 133 //clockwise, to angleC = 135
smilestone520 2:4988d0424f29 134
smilestone520 2:4988d0424f29 135 }
smilestone520 2:4988d0424f29 136 else //上邊界
smilestone520 2:4988d0424f29 137 {
smilestone520 2:4988d0424f29 138 if(angleC = -90 || angleC = 90)
smilestone520 2:4988d0424f29 139 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 140 else
smilestone520 4:ae47210fa346 141 {smallAngle(90);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 142 }
smilestone520 2:4988d0424f29 143
smilestone520 2:4988d0424f29 144 }
smilestone520 2:4988d0424f29 145 else if(yC < yB2) // 靠近下邊界
smilestone520 2:4988d0424f29 146 {
smilestone520 2:4988d0424f29 147 if(xC < xB3) //左下角
smilestone520 2:4988d0424f29 148 {
smilestone520 4:ae47210fa346 149 if(angleC <0 && angleC >=-90) // I
smilestone520 2:4988d0424f29 150
smilestone520 2:4988d0424f29 151 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 152
smilestone520 4:ae47210fa346 153 else if(angleC > -180 && angleC <-90) // II
smilestone520 2:4988d0424f29 154
smilestone520 4:ae47210fa346 155 //clockwise, to angleC = -45
smilestone520 2:4988d0424f29 156
smilestone520 4:ae47210fa346 157 else if(angleC <=180 && angleC >=90) // III
smilestone520 2:4988d0424f29 158
smilestone520 2:4988d0424f29 159 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 160
smilestone520 4:ae47210fa346 161 else if(angleC < 90 && angleC >=0) // IV
smilestone520 2:4988d0424f29 162
smilestone520 4:ae47210fa346 163 //counter clockwise, to angleC = -45
smilestone520 2:4988d0424f29 164
smilestone520 2:4988d0424f29 165 }
smilestone520 2:4988d0424f29 166 else if(xC > xB4) //右下角
smilestone520 2:4988d0424f29 167 {
smilestone520 4:ae47210fa346 168 if(angleC <0 && angleC >=-90) // I
smilestone520 2:4988d0424f29 169
smilestone520 4:ae47210fa346 170 //counter clockwise, to angleC = -135
smilestone520 2:4988d0424f29 171
smilestone520 4:ae47210fa346 172 else if(angleC > -180 && angleC <-90) // II
smilestone520 2:4988d0424f29 173
smilestone520 2:4988d0424f29 174 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 175
smilestone520 4:ae47210fa346 176 else if(angleC <=180 && angleC >=90) // III
smilestone520 2:4988d0424f29 177
smilestone520 4:ae47210fa346 178 //clockwise, to angleC = -135
smilestone520 2:4988d0424f29 179
smilestone520 4:ae47210fa346 180 else if(angleC < 90 && angleC >=0) // IV
smilestone520 2:4988d0424f29 181
smilestone520 2:4988d0424f29 182 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 183
smilestone520 2:4988d0424f29 184 }
smilestone520 2:4988d0424f29 185 else //下邊界
smilestone520 2:4988d0424f29 186 {
smilestone520 2:4988d0424f29 187 if(angleC = -90 || angleC = 90)
smilestone520 2:4988d0424f29 188 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 189 else
smilestone520 4:ae47210fa346 190 {smallAngle(-90);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 191 }
smilestone520 2:4988d0424f29 192
smilestone520 2:4988d0424f29 193 else if(xC < xB3) //靠近左邊界
smilestone520 2:4988d0424f29 194 {
smilestone520 2:4988d0424f29 195 if(angleC = 0 || angleC = 180)
smilestone520 2:4988d0424f29 196 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 197 else
smilestone520 2:4988d0424f29 198 {smallAngle(0);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 199 }
smilestone520 2:4988d0424f29 200
smilestone520 2:4988d0424f29 201 else if(xC > xB4) //靠近右邊界
smilestone520 2:4988d0424f29 202 {
smilestone520 2:4988d0424f29 203 if(angleC = 0 || angleC = 180)
smilestone520 2:4988d0424f29 204 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 205 else
smilestone520 2:4988d0424f29 206 {smallAngle(180);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 207 }
smilestone520 2:4988d0424f29 208 }
smilestone520 2:4988d0424f29 209
smilestone520 2:4988d0424f29 210 **************** smallAngle() *****************************************
smilestone520 2:4988d0424f29 211
smilestone520 2:4988d0424f29 212 smallAngle( float goodAngle ) // use the smallest turn to right angle ccw / cw, based on 0~360 degree system
smilestone520 2:4988d0424f29 213 {
smilestone520 4:ae47210fa346 214 if(angleR > 0) // CW
smilestone520 2:4988d0424f29 215 {
smilestone520 2:4988d0424f29 216 // turn clockwise to goodAngle;
smilestone520 2:4988d0424f29 217 }
smilestone520 4:ae47210fa346 218 else if(angleR < 0) //CCW
smilestone520 2:4988d0424f29 219 {
smilestone520 2:4988d0424f29 220 // turn counter clockwise to goodAngle;
smilestone520 2:4988d0424f29 221 }
smilestone520 2:4988d0424f29 222 }
smilestone520 2:4988d0424f29 223
smilestone520 2:4988d0424f29 224 *************** turnCW() ***********************************
smilestone520 2:4988d0424f29 225
smilestone520 2:4988d0424f29 226 turnCW(float goodAngle)
smilestone520 2:4988d0424f29 227 {
smilestone520 2:4988d0424f29 228 // turn clockwise to goodAngle;
smilestone520 2:4988d0424f29 229 }
smilestone520 2:4988d0424f29 230
smilestone520 2:4988d0424f29 231 *************** turnCCW() ***********************************
smilestone520 2:4988d0424f29 232
smilestone520 2:4988d0424f29 233 turnCCW(float goodAngle)
smilestone520 2:4988d0424f29 234 {
smilestone520 2:4988d0424f29 235 // turn counter clockwise to goodAngle;
smilestone520 2:4988d0424f29 236 }
smilestone520 2:4988d0424f29 237
smilestone520 2:4988d0424f29 238
smilestone520 6:f4250d71f974 239 ********** getBall() ********************88
smilestone520 6:f4250d71f974 240
smilestone520 6:f4250d71f974 241 getBall()
smilestone520 6:f4250d71f974 242 {
smilestone520 7:1232c7939984 243 // keep update car's and ball's position data from bluetooth
smilestone520 6:f4250d71f974 244
smilestone520 6:f4250d71f974 245 // if angleR is too much, stop , aim the direction, then move again
smilestone520 6:f4250d71f974 246
smilestone520 6:f4250d71f974 247 // when ball in the hunt region, activate the claw to get the ball
smilestone520 6:f4250d71f974 248
smilestone520 6:f4250d71f974 249 // check if ball is inside the region ,
smilestone520 6:f4250d71f974 250
smilestone520 7:1232c7939984 251 // Yes --> next state case 4
smilestone520 6:f4250d71f974 252
smilestone520 6:f4250d71f974 253 // No --> take back the claw
smilestone520 6:f4250d71f974 254
smilestone520 6:f4250d71f974 255 // back to case 1
smilestone520 6:f4250d71f974 256
smilestone520 6:f4250d71f974 257
smilestone520 6:f4250d71f974 258 }
smilestone520 2:4988d0424f29 259
smilestone520 7:1232c7939984 260 ******* moveToGate() ***************************
smilestone520 7:1232c7939984 261
smilestone520 7:1232c7939984 262 moveToGate()
smilestone520 7:1232c7939984 263 {
smilestone520 7:1232c7939984 264
smilestone520 7:1232c7939984 265
smilestone520 7:1232c7939984 266
smilestone520 7:1232c7939984 267 }
smilestone520 7:1232c7939984 268
smilestone520 2:4988d0424f29 269
smilestone520 2:4988d0424f29 270
smilestone520 2:4988d0424f29 271
smilestone520 2:4988d0424f29 272
smilestone520 2:4988d0424f29 273
smilestone520 2:4988d0424f29 274
smilestone520 2:4988d0424f29 275
smilestone520 2:4988d0424f29 276
smilestone520 2:4988d0424f29 277
smilestone520 2:4988d0424f29 278
smilestone520 2:4988d0424f29 279
smilestone520 2:4988d0424f29 280
smilestone520 2:4988d0424f29 281
smilestone520 2:4988d0424f29 282
smilestone520 2:4988d0424f29 283
smilestone520 2:4988d0424f29 284
smilestone520 2:4988d0424f29 285
smilestone520 2:4988d0424f29 286
smilestone520 2:4988d0424f29 287
smilestone520 2:4988d0424f29 288
smilestone520 2:4988d0424f29 289
smilestone520 2:4988d0424f29 290
smilestone520 2:4988d0424f29 291
smilestone520 2:4988d0424f29 292