Buggy bois / Mbed 2 deprecated UDITTEST

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

Robot.h@9:cefa177c1353, 2019-04-05 (annotated)

Committer:
mazdo25
Date:
Fri Apr 05 16:11:28 2019 +0000
Revision:
9:cefa177c1353
Parent:
8:5ed6685f6edd
Child:
10:ccf6155e3f97
law

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mazdo25 8:5ed6685f6edd 1 class Robot {
mazdo25 3:01b5e80d842d 2 private:
mazdo25 7:cb07cdb35b6c 3 float static const distanceBetweenWheels = 0.19; //currently not used may be used later
mazdo25 7:cb07cdb35b6c 4 int static const numberOfSamples = 100; //number of samples the line voltage array will hold
mazdo25 7:cb07cdb35b6c 5 int static const numberOfSensors = 6; //how many sensors you want to use
mazdo25 7:cb07cdb35b6c 6 float AF; //attenuation factor just a number you want to minimize max speed by, obviously dont go higher than 1
mazdo25 7:cb07cdb35b6c 7 Wheel* leftWheel; //a pointer to the left wheel
mazdo25 5:f1613df66ceb 8 Wheel* rightWheel;
mazdo25 3:01b5e80d842d 9 lineSensor* sensorArray[numberOfSensors];
mazdo25 5:f1613df66ceb 10 PID2 controller;
mazdo25 3:01b5e80d842d 11 float lineVoltages[numberOfSamples];
mazdo25 3:01b5e80d842d 12 int lvIndex;
mazdo25 3:01b5e80d842d 13 int sensorNumber;
mazdo25 6:477382219bcf 14
mazdo25 3:01b5e80d842d 15 Ticker updater;
mazdo25 8:5ed6685f6edd 16 Timeout timeToStop;
mazdo25 6:477382219bcf 17
mazdo25 8:5ed6685f6edd 18 float endOfLineDetection;
mazdo25 6:477382219bcf 19 float Reflec;
mazdo25 3:01b5e80d842d 20
mazdo25 4:208f5279143a 21 float RoboticAngularVelocity;
mazdo25 4:208f5279143a 22
mazdo25 7:cb07cdb35b6c 23 char state;
mazdo25 7:cb07cdb35b6c 24
mazdo25 3:01b5e80d842d 25 public:
mazdo25 3:01b5e80d842d 26
mazdo25 9:cefa177c1353 27 Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(2.0f, 0.0f, 0.0f, 0.0003f) //this controller actually does nothing atm
mazdo25 3:01b5e80d842d 28 {
mazdo25 6:477382219bcf 29 updater.detach();
mazdo25 8:5ed6685f6edd 30 timeToStop.detach();
mazdo25 9:cefa177c1353 31 AF = 0.7f; //change this value to change max speedzzzzzzzzzzzzzzzzz
mazdo25 8:5ed6685f6edd 32 state = 'F';
mazdo25 8:5ed6685f6edd 33 Reflec = 0;
mazdo25 8:5ed6685f6edd 34
mazdo25 5:f1613df66ceb 35 lvIndex = 0;
mazdo25 4:208f5279143a 36 sensorNumber = 0;
mazdo25 3:01b5e80d842d 37 sensorArray[0] = SA[0];
mazdo25 4:208f5279143a 38 sensorArray[1] = SA[1];
mazdo25 4:208f5279143a 39 sensorArray[2] = SA[2];
mazdo25 4:208f5279143a 40 sensorArray[3] = SA[3];
mazdo25 4:208f5279143a 41 sensorArray[4] = SA[4];
mazdo25 4:208f5279143a 42 sensorArray[5] = SA[5];
mazdo25 3:01b5e80d842d 43 leftWheel = LW;
mazdo25 3:01b5e80d842d 44 rightWheel= RW;
mazdo25 3:01b5e80d842d 45
mazdo25 4:208f5279143a 46 lineVoltages[lvIndex] = 0.0f;
mazdo25 8:5ed6685f6edd 47
mazdo25 9:cefa177c1353 48 controller.setInputLimits(-180.0f,180.0f);
mazdo25 8:5ed6685f6edd 49 controller.setSetPoint(0.0f);
mazdo25 3:01b5e80d842d 50 //controller will output a value between +- max speed of wheels
mazdo25 5:f1613df66ceb 51
mazdo25 5:f1613df66ceb 52 controller.assignLimitAddress(rightWheel->returnMinAVptr(),leftWheel->returnMaxAVptr());
mazdo25 3:01b5e80d842d 53 };
mazdo25 3:01b5e80d842d 54
mazdo25 3:01b5e80d842d 55 float calculateTranslationalVelocity()
mazdo25 3:01b5e80d842d 56 {
mazdo25 3:01b5e80d842d 57 return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
mazdo25 3:01b5e80d842d 58 }
mazdo25 4:208f5279143a 59
mazdo25 4:208f5279143a 60 //difference between angular velocities.
mazdo25 4:208f5279143a 61 void dW()
mazdo25 3:01b5e80d842d 62 {
mazdo25 4:208f5279143a 63 RoboticAngularVelocity = leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity();
mazdo25 4:208f5279143a 64 }
mazdo25 4:208f5279143a 65
mazdo25 4:208f5279143a 66 float returnRoboticAngularVelocity()
mazdo25 4:208f5279143a 67 {
mazdo25 4:208f5279143a 68 return RoboticAngularVelocity;
mazdo25 3:01b5e80d842d 69 }
mazdo25 3:01b5e80d842d 70
mazdo25 3:01b5e80d842d 71 //attempts to modify the angular velocity of the buggy
mazdo25 8:5ed6685f6edd 72 void adjustRbtAngularVelocity(float W)
mazdo25 3:01b5e80d842d 73 {
mazdo25 4:208f5279143a 74 //negative is a right turn
mazdo25 3:01b5e80d842d 75 if (W < 0)
mazdo25 3:01b5e80d842d 76 {
mazdo25 5:f1613df66ceb 77 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
mazdo25 5:f1613df66ceb 78 leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()+W)*AF);
mazdo25 3:01b5e80d842d 79 }
mazdo25 3:01b5e80d842d 80 else
mazdo25 3:01b5e80d842d 81 {
mazdo25 4:208f5279143a 82 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
mazdo25 5:f1613df66ceb 83 rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()-W)*AF);
mazdo25 4:208f5279143a 84 }
mazdo25 3:01b5e80d842d 85 }
mazdo25 3:01b5e80d842d 86
mazdo25 3:01b5e80d842d 87 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 88 {
mazdo25 8:5ed6685f6edd 89 switch (state)
mazdo25 8:5ed6685f6edd 90 {
mazdo25 8:5ed6685f6edd 91 case 'T':
mazdo25 8:5ed6685f6edd 92 turn180();
mazdo25 4:208f5279143a 93 break;
mazdo25 8:5ed6685f6edd 94 case 'S':
mazdo25 8:5ed6685f6edd 95 stopMovement();
mazdo25 8:5ed6685f6edd 96 break;
mazdo25 8:5ed6685f6edd 97 default:
mazdo25 8:5ed6685f6edd 98 followLine();
mazdo25 8:5ed6685f6edd 99 break;
mazdo25 8:5ed6685f6edd 100 }
mazdo25 3:01b5e80d842d 101 }
mazdo25 3:01b5e80d842d 102
mazdo25 3:01b5e80d842d 103 void stopMovement(void)
mazdo25 3:01b5e80d842d 104 {
mazdo25 8:5ed6685f6edd 105 leftWheel->adjustAngularVelocity(0.0f);
mazdo25 8:5ed6685f6edd 106 rightWheel->adjustAngularVelocity(0.0f);
mazdo25 3:01b5e80d842d 107 }
mazdo25 5:f1613df66ceb 108
mazdo25 5:f1613df66ceb 109 float returnLineVoltage()
mazdo25 5:f1613df66ceb 110 {
mazdo25 5:f1613df66ceb 111 return lineVoltages[lvIndex%numberOfSamples];
mazdo25 5:f1613df66ceb 112 }
mazdo25 5:f1613df66ceb 113
mazdo25 5:f1613df66ceb 114 void rbtInit()
mazdo25 5:f1613df66ceb 115 {
mazdo25 6:477382219bcf 116 sensorArray[0]->sample();
mazdo25 8:5ed6685f6edd 117 sensorArray[0]->calcLineVoltage();
mazdo25 8:5ed6685f6edd 118 Reflec = sensorArray[0]->returnLineVoltage();
mazdo25 8:5ed6685f6edd 119 updater.attach(callback(this, &Robot::robotUpdates),0.0001f);
mazdo25 5:f1613df66ceb 120 }
mazdo25 7:cb07cdb35b6c 121
mazdo25 7:cb07cdb35b6c 122 void spin()
mazdo25 7:cb07cdb35b6c 123 {
mazdo25 7:cb07cdb35b6c 124 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.3f);
mazdo25 7:cb07cdb35b6c 125 leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel())*-0.3f);
mazdo25 7:cb07cdb35b6c 126 }
mazdo25 7:cb07cdb35b6c 127
mazdo25 7:cb07cdb35b6c 128 void setState(char S)
mazdo25 7:cb07cdb35b6c 129 {
mazdo25 7:cb07cdb35b6c 130 state = S;
mazdo25 7:cb07cdb35b6c 131 }
mazdo25 8:5ed6685f6edd 132
mazdo25 8:5ed6685f6edd 133 void followLine(void)
mazdo25 8:5ed6685f6edd 134 {
mazdo25 8:5ed6685f6edd 135 float ambientLight;
mazdo25 8:5ed6685f6edd 136 float reading;
mazdo25 8:5ed6685f6edd 137 ambientLight = sensorArray[sensorNumber]->calcLineVoltage();
mazdo25 8:5ed6685f6edd 138 sensorArray[sensorNumber]->sample();
mazdo25 8:5ed6685f6edd 139 sensorArray[sensorNumber]->calcLineVoltage();
mazdo25 8:5ed6685f6edd 140 reading = (sensorArray[sensorNumber]->returnLineVoltage()-ambientLight);
mazdo25 8:5ed6685f6edd 141 switch (sensorNumber)
mazdo25 8:5ed6685f6edd 142 {
mazdo25 8:5ed6685f6edd 143 case 0:
mazdo25 9:cefa177c1353 144 lineVoltages[lvIndex%numberOfSamples] = (reading * -500.0f);
mazdo25 9:cefa177c1353 145 if (reading <= Reflec*4){endOfLineDetection++;}
mazdo25 9:cefa177c1353 146
mazdo25 8:5ed6685f6edd 147 break;
mazdo25 8:5ed6685f6edd 148 case 1:
mazdo25 9:cefa177c1353 149 lineVoltages[lvIndex%numberOfSamples] += (reading * -180.0f);
mazdo25 9:cefa177c1353 150 if (reading <= Reflec*4) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 151 break;
mazdo25 8:5ed6685f6edd 152 case 2:
mazdo25 9:cefa177c1353 153 lineVoltages[lvIndex%numberOfSamples] += (reading * -50.0f);
mazdo25 9:cefa177c1353 154 if (reading <= Reflec*4) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 155 break;
mazdo25 8:5ed6685f6edd 156 case 3:
mazdo25 9:cefa177c1353 157 lineVoltages[lvIndex%numberOfSamples] += (reading * 50.0f);
mazdo25 9:cefa177c1353 158 if (reading <= Reflec*4) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 159 break;
mazdo25 8:5ed6685f6edd 160 case 4:
mazdo25 9:cefa177c1353 161 lineVoltages[lvIndex%numberOfSamples] += (reading * 150.0f);
mazdo25 9:cefa177c1353 162 if (reading <= Reflec*4) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 163 break;
mazdo25 8:5ed6685f6edd 164 case 5:
mazdo25 9:cefa177c1353 165 lineVoltages[lvIndex%numberOfSamples] += (reading * 250.0f);
mazdo25 9:cefa177c1353 166 if (reading <= Reflec*4) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 167 break;
mazdo25 8:5ed6685f6edd 168 }
mazdo25 8:5ed6685f6edd 169
mazdo25 8:5ed6685f6edd 170 sensorNumber ++;
mazdo25 8:5ed6685f6edd 171
mazdo25 8:5ed6685f6edd 172 if (sensorNumber >= numberOfSensors)
mazdo25 8:5ed6685f6edd 173 {
mazdo25 8:5ed6685f6edd 174 sensorNumber = 0;
mazdo25 9:cefa177c1353 175 if (endOfLineDetection < 8)
mazdo25 8:5ed6685f6edd 176 {
mazdo25 9:cefa177c1353 177 AF = scaler(-180.0f,180.0f, 0.2f, 1.0f, lineVoltages[lvIndex%numberOfSamples]);
mazdo25 8:5ed6685f6edd 178 adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
mazdo25 8:5ed6685f6edd 179 lvIndex++;
mazdo25 8:5ed6685f6edd 180 }
mazdo25 8:5ed6685f6edd 181 else if (AF > 0.1f)
mazdo25 8:5ed6685f6edd 182 {
mazdo25 9:cefa177c1353 183 AF -= AF/2.0f;
mazdo25 9:cefa177c1353 184 adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples-1]);
mazdo25 8:5ed6685f6edd 185 }else{
mazdo25 8:5ed6685f6edd 186 AF = 0.0f;
mazdo25 8:5ed6685f6edd 187 stopMovement();
mazdo25 8:5ed6685f6edd 188 }
mazdo25 8:5ed6685f6edd 189 endOfLineDetection = 0;
mazdo25 8:5ed6685f6edd 190 }
mazdo25 8:5ed6685f6edd 191 }
mazdo25 8:5ed6685f6edd 192
mazdo25 8:5ed6685f6edd 193 void turn180()
mazdo25 8:5ed6685f6edd 194 {
mazdo25 8:5ed6685f6edd 195 updater.detach();
mazdo25 8:5ed6685f6edd 196 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.3f);
mazdo25 8:5ed6685f6edd 197 leftWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*-0.3f);
mazdo25 8:5ed6685f6edd 198 timeToStop.attach(callback(this, &Robot::reAttach),1.3f);
mazdo25 8:5ed6685f6edd 199 state = 'S';
mazdo25 8:5ed6685f6edd 200 }
mazdo25 8:5ed6685f6edd 201
mazdo25 8:5ed6685f6edd 202 void reAttach()
mazdo25 8:5ed6685f6edd 203 {
mazdo25 9:cefa177c1353 204 updater.attach(callback(this, &Robot::robotUpdates),0.00005f);
mazdo25 9:cefa177c1353 205 }
mazdo25 9:cefa177c1353 206
mazdo25 9:cefa177c1353 207 float scaler(float prevMin, float prevMax, float newMin, float newMax, float var)
mazdo25 9:cefa177c1353 208 {
mazdo25 9:cefa177c1353 209 if (var > prevMax) {var = prevMax;}
mazdo25 9:cefa177c1353 210 if (var < prevMin) {var = prevMin;}
mazdo25 9:cefa177c1353 211 return (((var-prevMin)/(prevMax - prevMin))*(newMax-newMin))+newMin;
mazdo25 8:5ed6685f6edd 212 }
mazdo25 3:01b5e80d842d 213
mazdo25 3:01b5e80d842d 214 };
mazdo25 3:01b5e80d842d 215
mazdo25 3:01b5e80d842d 216 /*
mazdo25 3:01b5e80d842d 217 Timeout timeToStop,
mazdo25 8:5ed6685f6edd 218
mazdo25 3:01b5e80d842d 219
mazdo25 3:01b5e80d842d 220 void travelDistance(float d)//in metres
mazdo25 3:01b5e80d842d 221 {
mazdo25 3:01b5e80d842d 222 timeToStop.attach(callback(this, &Robot::stopMovement), d/(calculateTranslationalVelocity()*(float)PI*Wheel::wheelDiameter));
mazdo25 3:01b5e80d842d 223 }
mazdo25 3:01b5e80d842d 224
mazdo25 3:01b5e80d842d 225 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 226 {
mazdo25 3:01b5e80d842d 227 sensorArray[sensorNumber]->sample();
mazdo25 3:01b5e80d842d 228
mazdo25 3:01b5e80d842d 229 if (sensorNumber < (numberOfSensors/2))
mazdo25 3:01b5e80d842d 230 {
mazdo25 3:01b5e80d842d 231 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-3);
mazdo25 3:01b5e80d842d 232 }
mazdo25 3:01b5e80d842d 233 else
mazdo25 3:01b5e80d842d 234 }
mazdo25 3:01b5e80d842d 235 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-2);
mazdo25 3:01b5e80d842d 236 }
mazdo25 3:01b5e80d842d 237
mazdo25 3:01b5e80d842d 238 sensorNumber++;
mazdo25 3:01b5e80d842d 239 if (sensorNumber % numberOfSensors == 0)
mazdo25 3:01b5e80d842d 240 {
mazdo25 3:01b5e80d842d 241 sensorNumber = 0;
mazdo25 3:01b5e80d842d 242 controller.setProcessValue(lineVoltages[(lvIndex%numberOfSamples)];
mazdo25 3:01b5e80d842d 243 adjustAngularVelocity(controller.compute());
mazdo25 3:01b5e80d842d 244 lvIndex++;
mazdo25 3:01b5e80d842d 245 }
mazdo25 3:01b5e80d842d 246 }
mazdo25 3:01b5e80d842d 247
mazdo25 3:01b5e80d842d 248 */
mazdo25 4:208f5279143a 249 //lineVoltages[lvIndex] += 0.5f;
mazdo25 4:208f5279143a 250
mazdo25 4:208f5279143a 251
mazdo25 4:208f5279143a 252
mazdo25 4:208f5279143a 253 /*if input value is greater than the maximum value the left wheel can go, go full right TURN
mazdo25 4:208f5279143a 254 if (W > leftWheel->returnMaxAngularVel()*AF)
mazdo25 4:208f5279143a 255 {
mazdo25 4:208f5279143a 256 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 257 rightWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 258 }
mazdo25 4:208f5279143a 259 else if (W < (-1.0f*rightWheel->returnMaxAngularVel())) //if input value is less than the fastest the right wheel can go in reverse go full left TURN
mazdo25 4:208f5279143a 260 {
mazdo25 4:208f5279143a 261 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 262 leftWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 263 }
mazdo25 4:208f5279143a 264 else if (W == 0)
mazdo25 4:208f5279143a 265 {
mazdo25 4:208f5279143a 266 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 267 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 268 }
mazdo25 4:208f5279143a 269 else
mazdo25 4:208f5279143a 270 {
mazdo25 4:208f5279143a 271
mazdo25 4:208f5279143a 272 */