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@3:9f5346fc58b3, 2016-05-24 (annotated)

Committer:
smilestone520
Date:
Tue May 24 06:43:06 2016 +0000
Revision:
3:9f5346fc58b3
Parent:
2:4988d0424f29
Child:
4:ae47210fa346
hahah

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 2:4988d0424f29 10 float angleR // relative angle between car-nextSpot angle and car's angle, angleP - angle C, based on 0~360 degree system
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 2:4988d0424f29 22 ***** Code Begin **********
smilestone520 2:4988d0424f29 23
smilestone520 2:4988d0424f29 24 yB1 yB2 xB3 xB4 // broder conditions
smilestone520 2:4988d0424f29 25
smilestone520 2:4988d0424f29 26 yB1 = 0-longC
smilestone520 2:4988d0424f29 27
smilestone520 2:4988d0424f29 28 yB2 = 0-wideB + longC
smilestone520 2:4988d0424f29 29
smilestone520 2:4988d0424f29 30 xB3 = longC
smilestone520 2:4988d0424f29 31
smilestone520 2:4988d0424f29 32 xB4 = longB-longC
smilestone520 2:4988d0424f29 33 *******************************************************************************
smilestone520 2:4988d0424f29 34
smilestone520 2:4988d0424f29 35 遠離邊界
smilestone520 2:4988d0424f29 36
smilestone520 3:9f5346fc58b3 37 car: away from the border --> turn to the ball direction --> to get the ball --> get ball
smilestone520 3:9f5346fc58b3 38 ---> turn to the point before the gate ---> to the point before the gate --> turn to the gate direction
smilestone520 3:9f5346fc58b3 39 ---> to the gate --> release the ball
smilestone520 2:4988d0424f29 40
smilestone520 2:4988d0424f29 41
smilestone520 2:4988d0424f29 42
smilestone520 2:4988d0424f29 43 matlab:
smilestone520 2:4988d0424f29 44
smilestone520 2:4988d0424f29 45
smilestone520 2:4988d0424f29 46
smilestone520 2:4988d0424f29 47
smilestone520 2:4988d0424f29 48 ********************************************************************************
smilestone520 2:4988d0424f29 49
smilestone520 2:4988d0424f29 50 **************** funcBorder() *****************************************
smilestone520 2:4988d0424f29 51
smilestone520 2:4988d0424f29 52 funcBorder() // check if car is too close to the border
smilestone520 2:4988d0424f29 53
smilestone520 2:4988d0424f29 54 if(yC > yB1 || yC < yB2 || xC < xB3 || xC > xB4){
smilestone520 2:4988d0424f29 55
smilestone520 2:4988d0424f29 56 if(yC > yB1) // 靠近上邊界
smilestone520 2:4988d0424f29 57 {
smilestone520 2:4988d0424f29 58 if(xC < xB3) //左上角
smilestone520 2:4988d0424f29 59 {
smilestone520 2:4988d0424f29 60 if(angleC <= 90 && angleC >0) // I
smilestone520 2:4988d0424f29 61
smilestone520 2:4988d0424f29 62 //clockwise, to angleC = -45
smilestone520 2:4988d0424f29 63
smilestone520 2:4988d0424f29 64 else if(angleC <= 180 && angleC >90) // II
smilestone520 2:4988d0424f29 65
smilestone520 2:4988d0424f29 66 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 67
smilestone520 2:4988d0424f29 68 else if(angleC <= 270 && angleC 180) // III
smilestone520 2:4988d0424f29 69
smilestone520 2:4988d0424f29 70 //counter clockwise, to angleC = -45
smilestone520 2:4988d0424f29 71
smilestone520 2:4988d0424f29 72 else if(angleC <= 360 && angleC 270) // IV
smilestone520 2:4988d0424f29 73
smilestone520 2:4988d0424f29 74 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 75
smilestone520 2:4988d0424f29 76 }
smilestone520 2:4988d0424f29 77 else if(xC > xB4) //右上角
smilestone520 2:4988d0424f29 78 {
smilestone520 2:4988d0424f29 79 if(angleC <= 90 && angleC >0) // I
smilestone520 2:4988d0424f29 80
smilestone520 2:4988d0424f29 81 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 82
smilestone520 2:4988d0424f29 83 else if(angleC <= 180 && angleC >90) // II
smilestone520 2:4988d0424f29 84
smilestone520 2:4988d0424f29 85 //counter clockwise, to angleC = -135
smilestone520 2:4988d0424f29 86
smilestone520 2:4988d0424f29 87 else if(angleC <= 270 && angleC 180) // III
smilestone520 2:4988d0424f29 88
smilestone520 2:4988d0424f29 89 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 90
smilestone520 2:4988d0424f29 91 else if(angleC <= 360 && angleC 270) // IV
smilestone520 2:4988d0424f29 92
smilestone520 2:4988d0424f29 93 //clockwise, to angleC = -135
smilestone520 2:4988d0424f29 94
smilestone520 2:4988d0424f29 95 }
smilestone520 2:4988d0424f29 96 else //上邊界
smilestone520 2:4988d0424f29 97 {
smilestone520 2:4988d0424f29 98 if(angleC = -90 || angleC = 90)
smilestone520 2:4988d0424f29 99 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 100 else
smilestone520 2:4988d0424f29 101 {smallAngle(-90);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 102 }
smilestone520 2:4988d0424f29 103
smilestone520 2:4988d0424f29 104 }
smilestone520 2:4988d0424f29 105 else if(yC < yB2) // 靠近下邊界
smilestone520 2:4988d0424f29 106 {
smilestone520 2:4988d0424f29 107 if(xC < xB3) //左下角
smilestone520 2:4988d0424f29 108 {
smilestone520 2:4988d0424f29 109 if(angleC <= 90 && angleC >0) // I
smilestone520 2:4988d0424f29 110
smilestone520 2:4988d0424f29 111 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 112
smilestone520 2:4988d0424f29 113 else if(angleC <= 180 && angleC >90) // II
smilestone520 2:4988d0424f29 114
smilestone520 2:4988d0424f29 115 //clockwise, to angleC = 45
smilestone520 2:4988d0424f29 116
smilestone520 2:4988d0424f29 117 else if(angleC <= 270 && angleC 180) // III
smilestone520 2:4988d0424f29 118
smilestone520 2:4988d0424f29 119 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 120
smilestone520 2:4988d0424f29 121 else if(angleC <= 360 && angleC 270) // IV
smilestone520 2:4988d0424f29 122
smilestone520 2:4988d0424f29 123 //counter clockwise, to angleC = 45
smilestone520 2:4988d0424f29 124
smilestone520 2:4988d0424f29 125 }
smilestone520 2:4988d0424f29 126 else if(xC > xB4) //右下角
smilestone520 2:4988d0424f29 127 {
smilestone520 2:4988d0424f29 128 if(angleC <= 90 && angleC >0) // I
smilestone520 2:4988d0424f29 129
smilestone520 2:4988d0424f29 130 //counter clockwise, to angleC = 135
smilestone520 2:4988d0424f29 131
smilestone520 2:4988d0424f29 132 else if(angleC <= 180 && angleC >90) // II
smilestone520 2:4988d0424f29 133
smilestone520 2:4988d0424f29 134 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 135
smilestone520 2:4988d0424f29 136 else if(angleC <= 270 && angleC 180) // III
smilestone520 2:4988d0424f29 137
smilestone520 2:4988d0424f29 138 //clockwise, to angleC = 135
smilestone520 2:4988d0424f29 139
smilestone520 2:4988d0424f29 140 else if(angleC <= 360 && angleC 270) // IV
smilestone520 2:4988d0424f29 141
smilestone520 2:4988d0424f29 142 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 143
smilestone520 2:4988d0424f29 144 }
smilestone520 2:4988d0424f29 145 else //下邊界
smilestone520 2:4988d0424f29 146 {
smilestone520 2:4988d0424f29 147 if(angleC = -90 || angleC = 90)
smilestone520 2:4988d0424f29 148 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 149 else
smilestone520 2:4988d0424f29 150 {smallAngle(90);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 151 }
smilestone520 2:4988d0424f29 152
smilestone520 2:4988d0424f29 153 else if(xC < xB3) //靠近左邊界
smilestone520 2:4988d0424f29 154 {
smilestone520 2:4988d0424f29 155 if(angleC = 0 || angleC = 180)
smilestone520 2:4988d0424f29 156 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 157 else
smilestone520 2:4988d0424f29 158 {smallAngle(0);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 159 }
smilestone520 2:4988d0424f29 160
smilestone520 2:4988d0424f29 161 else if(xC > xB4) //靠近右邊界
smilestone520 2:4988d0424f29 162 {
smilestone520 2:4988d0424f29 163 if(angleC = 0 || angleC = 180)
smilestone520 2:4988d0424f29 164 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 165 else
smilestone520 2:4988d0424f29 166 {smallAngle(180);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 167 }
smilestone520 2:4988d0424f29 168 }
smilestone520 2:4988d0424f29 169
smilestone520 2:4988d0424f29 170 **************** smallAngle() *****************************************
smilestone520 2:4988d0424f29 171
smilestone520 2:4988d0424f29 172 smallAngle( float goodAngle ) // use the smallest turn to right angle ccw / cw, based on 0~360 degree system
smilestone520 2:4988d0424f29 173 {
smilestone520 2:4988d0424f29 174 if(angleR - angleC <= 180 ) // CW
smilestone520 2:4988d0424f29 175 {
smilestone520 2:4988d0424f29 176 // turn clockwise to goodAngle;
smilestone520 2:4988d0424f29 177 }
smilestone520 2:4988d0424f29 178 else if(angleR - angleC > 180) //CCW
smilestone520 2:4988d0424f29 179 {
smilestone520 2:4988d0424f29 180 // turn counter clockwise to goodAngle;
smilestone520 2:4988d0424f29 181 }
smilestone520 2:4988d0424f29 182 }
smilestone520 2:4988d0424f29 183
smilestone520 2:4988d0424f29 184 *************** turnCW() ***********************************
smilestone520 2:4988d0424f29 185
smilestone520 2:4988d0424f29 186 turnCW(float goodAngle)
smilestone520 2:4988d0424f29 187 {
smilestone520 2:4988d0424f29 188 // turn clockwise to goodAngle;
smilestone520 2:4988d0424f29 189 }
smilestone520 2:4988d0424f29 190
smilestone520 2:4988d0424f29 191 *************** turnCCW() ***********************************
smilestone520 2:4988d0424f29 192
smilestone520 2:4988d0424f29 193 turnCCW(float goodAngle)
smilestone520 2:4988d0424f29 194 {
smilestone520 2:4988d0424f29 195 // turn counter clockwise to goodAngle;
smilestone520 2:4988d0424f29 196 }
smilestone520 2:4988d0424f29 197
smilestone520 2:4988d0424f29 198
smilestone520 2:4988d0424f29 199 **********
smilestone520 2:4988d0424f29 200
smilestone520 2:4988d0424f29 201
smilestone520 2:4988d0424f29 202
smilestone520 2:4988d0424f29 203
smilestone520 2:4988d0424f29 204
smilestone520 2:4988d0424f29 205
smilestone520 2:4988d0424f29 206
smilestone520 2:4988d0424f29 207
smilestone520 2:4988d0424f29 208
smilestone520 2:4988d0424f29 209
smilestone520 2:4988d0424f29 210
smilestone520 2:4988d0424f29 211
smilestone520 2:4988d0424f29 212
smilestone520 2:4988d0424f29 213
smilestone520 2:4988d0424f29 214
smilestone520 2:4988d0424f29 215
smilestone520 2:4988d0424f29 216
smilestone520 2:4988d0424f29 217
smilestone520 2:4988d0424f29 218
smilestone520 2:4988d0424f29 219
smilestone520 2:4988d0424f29 220
smilestone520 2:4988d0424f29 221
smilestone520 2:4988d0424f29 222
smilestone520 2:4988d0424f29 223
smilestone520 2:4988d0424f29 224