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@2:4988d0424f29, 2016-05-24 (annotated)

Committer:
smilestone520
Date:
Tue May 24 06:40:52 2016 +0000
Revision:
2:4988d0424f29
Parent:
1:a6e23913e047
Child:
3:9f5346fc58b3
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 2:4988d0424f29 37 car:
smilestone520 2:4988d0424f29 38
smilestone520 2:4988d0424f29 39
smilestone520 2:4988d0424f29 40
smilestone520 2:4988d0424f29 41 matlab:
smilestone520 2:4988d0424f29 42
smilestone520 2:4988d0424f29 43
smilestone520 2:4988d0424f29 44
smilestone520 2:4988d0424f29 45
smilestone520 2:4988d0424f29 46 ********************************************************************************
smilestone520 2:4988d0424f29 47
smilestone520 2:4988d0424f29 48 **************** funcBorder() *****************************************
smilestone520 2:4988d0424f29 49
smilestone520 2:4988d0424f29 50 funcBorder() // check if car is too close to the border
smilestone520 2:4988d0424f29 51
smilestone520 2:4988d0424f29 52 if(yC > yB1 || yC < yB2 || xC < xB3 || xC > xB4){
smilestone520 2:4988d0424f29 53
smilestone520 2:4988d0424f29 54 if(yC > yB1) // 靠近上邊界
smilestone520 2:4988d0424f29 55 {
smilestone520 2:4988d0424f29 56 if(xC < xB3) //左上角
smilestone520 2:4988d0424f29 57 {
smilestone520 2:4988d0424f29 58 if(angleC <= 90 && angleC >0) // I
smilestone520 2:4988d0424f29 59
smilestone520 2:4988d0424f29 60 //clockwise, to angleC = -45
smilestone520 2:4988d0424f29 61
smilestone520 2:4988d0424f29 62 else if(angleC <= 180 && angleC >90) // II
smilestone520 2:4988d0424f29 63
smilestone520 2:4988d0424f29 64 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 65
smilestone520 2:4988d0424f29 66 else if(angleC <= 270 && angleC 180) // III
smilestone520 2:4988d0424f29 67
smilestone520 2:4988d0424f29 68 //counter clockwise, to angleC = -45
smilestone520 2:4988d0424f29 69
smilestone520 2:4988d0424f29 70 else if(angleC <= 360 && angleC 270) // IV
smilestone520 2:4988d0424f29 71
smilestone520 2:4988d0424f29 72 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 73
smilestone520 2:4988d0424f29 74 }
smilestone520 2:4988d0424f29 75 else if(xC > xB4) //右上角
smilestone520 2:4988d0424f29 76 {
smilestone520 2:4988d0424f29 77 if(angleC <= 90 && angleC >0) // I
smilestone520 2:4988d0424f29 78
smilestone520 2:4988d0424f29 79 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 80
smilestone520 2:4988d0424f29 81 else if(angleC <= 180 && angleC >90) // II
smilestone520 2:4988d0424f29 82
smilestone520 2:4988d0424f29 83 //counter clockwise, to angleC = -135
smilestone520 2:4988d0424f29 84
smilestone520 2:4988d0424f29 85 else if(angleC <= 270 && angleC 180) // III
smilestone520 2:4988d0424f29 86
smilestone520 2:4988d0424f29 87 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 88
smilestone520 2:4988d0424f29 89 else if(angleC <= 360 && angleC 270) // IV
smilestone520 2:4988d0424f29 90
smilestone520 2:4988d0424f29 91 //clockwise, to angleC = -135
smilestone520 2:4988d0424f29 92
smilestone520 2:4988d0424f29 93 }
smilestone520 2:4988d0424f29 94 else //上邊界
smilestone520 2:4988d0424f29 95 {
smilestone520 2:4988d0424f29 96 if(angleC = -90 || angleC = 90)
smilestone520 2:4988d0424f29 97 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 98 else
smilestone520 2:4988d0424f29 99 {smallAngle(-90);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 100 }
smilestone520 2:4988d0424f29 101
smilestone520 2:4988d0424f29 102 }
smilestone520 2:4988d0424f29 103 else if(yC < yB2) // 靠近下邊界
smilestone520 2:4988d0424f29 104 {
smilestone520 2:4988d0424f29 105 if(xC < xB3) //左下角
smilestone520 2:4988d0424f29 106 {
smilestone520 2:4988d0424f29 107 if(angleC <= 90 && angleC >0) // I
smilestone520 2:4988d0424f29 108
smilestone520 2:4988d0424f29 109 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 110
smilestone520 2:4988d0424f29 111 else if(angleC <= 180 && angleC >90) // II
smilestone520 2:4988d0424f29 112
smilestone520 2:4988d0424f29 113 //clockwise, to angleC = 45
smilestone520 2:4988d0424f29 114
smilestone520 2:4988d0424f29 115 else if(angleC <= 270 && angleC 180) // III
smilestone520 2:4988d0424f29 116
smilestone520 2:4988d0424f29 117 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 118
smilestone520 2:4988d0424f29 119 else if(angleC <= 360 && angleC 270) // IV
smilestone520 2:4988d0424f29 120
smilestone520 2:4988d0424f29 121 //counter clockwise, to angleC = 45
smilestone520 2:4988d0424f29 122
smilestone520 2:4988d0424f29 123 }
smilestone520 2:4988d0424f29 124 else if(xC > xB4) //右下角
smilestone520 2:4988d0424f29 125 {
smilestone520 2:4988d0424f29 126 if(angleC <= 90 && angleC >0) // I
smilestone520 2:4988d0424f29 127
smilestone520 2:4988d0424f29 128 //counter clockwise, to angleC = 135
smilestone520 2:4988d0424f29 129
smilestone520 2:4988d0424f29 130 else if(angleC <= 180 && angleC >90) // II
smilestone520 2:4988d0424f29 131
smilestone520 2:4988d0424f29 132 //forward 0.5*longC, judge again
smilestone520 2:4988d0424f29 133
smilestone520 2:4988d0424f29 134 else if(angleC <= 270 && angleC 180) // III
smilestone520 2:4988d0424f29 135
smilestone520 2:4988d0424f29 136 //clockwise, to angleC = 135
smilestone520 2:4988d0424f29 137
smilestone520 2:4988d0424f29 138 else if(angleC <= 360 && angleC 270) // IV
smilestone520 2:4988d0424f29 139
smilestone520 2:4988d0424f29 140 //back 0.5*longC, judge again
smilestone520 2:4988d0424f29 141
smilestone520 2:4988d0424f29 142 }
smilestone520 2:4988d0424f29 143 else //下邊界
smilestone520 2:4988d0424f29 144 {
smilestone520 2:4988d0424f29 145 if(angleC = -90 || angleC = 90)
smilestone520 2:4988d0424f29 146 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 147 else
smilestone520 2:4988d0424f29 148 {smallAngle(90);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 149 }
smilestone520 2:4988d0424f29 150
smilestone520 2:4988d0424f29 151 else if(xC < xB3) //靠近左邊界
smilestone520 2:4988d0424f29 152 {
smilestone520 2:4988d0424f29 153 if(angleC = 0 || angleC = 180)
smilestone520 2:4988d0424f29 154 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 155 else
smilestone520 2:4988d0424f29 156 {smallAngle(0);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 157 }
smilestone520 2:4988d0424f29 158
smilestone520 2:4988d0424f29 159 else if(xC > xB4) //靠近右邊界
smilestone520 2:4988d0424f29 160 {
smilestone520 2:4988d0424f29 161 if(angleC = 0 || angleC = 180)
smilestone520 2:4988d0424f29 162 {} // do nothing, turn to next point
smilestone520 2:4988d0424f29 163 else
smilestone520 2:4988d0424f29 164 {smallAngle(180);} // turn to 90, then turn to next point
smilestone520 2:4988d0424f29 165 }
smilestone520 2:4988d0424f29 166 }
smilestone520 2:4988d0424f29 167
smilestone520 2:4988d0424f29 168 **************** smallAngle() *****************************************
smilestone520 2:4988d0424f29 169
smilestone520 2:4988d0424f29 170 smallAngle( float goodAngle ) // use the smallest turn to right angle ccw / cw, based on 0~360 degree system
smilestone520 2:4988d0424f29 171 {
smilestone520 2:4988d0424f29 172 if(angleR - angleC <= 180 ) // CW
smilestone520 2:4988d0424f29 173 {
smilestone520 2:4988d0424f29 174 // turn clockwise to goodAngle;
smilestone520 2:4988d0424f29 175 }
smilestone520 2:4988d0424f29 176 else if(angleR - angleC > 180) //CCW
smilestone520 2:4988d0424f29 177 {
smilestone520 2:4988d0424f29 178 // turn counter clockwise to goodAngle;
smilestone520 2:4988d0424f29 179 }
smilestone520 2:4988d0424f29 180 }
smilestone520 2:4988d0424f29 181
smilestone520 2:4988d0424f29 182 *************** turnCW() ***********************************
smilestone520 2:4988d0424f29 183
smilestone520 2:4988d0424f29 184 turnCW(float goodAngle)
smilestone520 2:4988d0424f29 185 {
smilestone520 2:4988d0424f29 186 // turn clockwise to goodAngle;
smilestone520 2:4988d0424f29 187 }
smilestone520 2:4988d0424f29 188
smilestone520 2:4988d0424f29 189 *************** turnCCW() ***********************************
smilestone520 2:4988d0424f29 190
smilestone520 2:4988d0424f29 191 turnCCW(float goodAngle)
smilestone520 2:4988d0424f29 192 {
smilestone520 2:4988d0424f29 193 // turn counter clockwise to goodAngle;
smilestone520 2:4988d0424f29 194 }
smilestone520 2:4988d0424f29 195
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