Buggy bois / Mbed 2 deprecated HEATS_1

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@11:78c645fb76cd, 2019-04-28 (annotated)

Committer:
mazdo25
Date:
Sun Apr 28 22:20:28 2019 +0000
Revision:
11:78c645fb76cd
Parent:
10:acf5cb8d58d5
Child:
12:11d203351eb0
WOw;

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