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@7:cb07cdb35b6c, 2019-03-28 (annotated)

Committer:
mazdo25
Date:
Thu Mar 28 01:07:54 2019 +0000
Revision:
7:cb07cdb35b6c
Parent:
6:477382219bcf
Child:
8:5ed6685f6edd
Latest, NOT tested version;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mazdo25 3:01b5e80d842d 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 6:477382219bcf 16
mazdo25 5:f1613df66ceb 17 int endOfLineDetection;
mazdo25 6:477382219bcf 18 float Reflec;
mazdo25 3:01b5e80d842d 19
mazdo25 4:208f5279143a 20 float RoboticAngularVelocity;
mazdo25 4:208f5279143a 21
mazdo25 7:cb07cdb35b6c 22 char state;
mazdo25 7:cb07cdb35b6c 23
mazdo25 3:01b5e80d842d 24 public:
mazdo25 3:01b5e80d842d 25
mazdo25 7:cb07cdb35b6c 26 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 27 {
mazdo25 6:477382219bcf 28 updater.detach();
mazdo25 7:cb07cdb35b6c 29 AF = 0.3f; //change this value to change max speedzzzzzzzzzzzzzzzzz
mazdo25 7:cb07cdb35b6c 30 state = 'S';
mazdo25 6:477382219bcf 31
mazdo25 6:477382219bcf 32 Reflec = 0;
mazdo25 5:f1613df66ceb 33 lvIndex = 0;
mazdo25 4:208f5279143a 34 sensorNumber = 0;
mazdo25 3:01b5e80d842d 35 sensorArray[0] = SA[0];
mazdo25 4:208f5279143a 36 sensorArray[1] = SA[1];
mazdo25 4:208f5279143a 37 sensorArray[2] = SA[2];
mazdo25 4:208f5279143a 38 sensorArray[3] = SA[3];
mazdo25 4:208f5279143a 39 sensorArray[4] = SA[4];
mazdo25 4:208f5279143a 40 sensorArray[5] = SA[5];
mazdo25 3:01b5e80d842d 41 leftWheel = LW;
mazdo25 3:01b5e80d842d 42 rightWheel= RW;
mazdo25 3:01b5e80d842d 43
mazdo25 4:208f5279143a 44 lineVoltages[lvIndex] = 0.0f;
mazdo25 3:01b5e80d842d 45 //controller will output a value between +- max speed of wheels
mazdo25 5:f1613df66ceb 46
mazdo25 5:f1613df66ceb 47 controller.assignLimitAddress(rightWheel->returnMinAVptr(),leftWheel->returnMaxAVptr());
mazdo25 3:01b5e80d842d 48 };
mazdo25 3:01b5e80d842d 49
mazdo25 3:01b5e80d842d 50 float calculateTranslationalVelocity()
mazdo25 3:01b5e80d842d 51 {
mazdo25 3:01b5e80d842d 52 return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
mazdo25 3:01b5e80d842d 53 }
mazdo25 4:208f5279143a 54
mazdo25 4:208f5279143a 55 //difference between angular velocities.
mazdo25 4:208f5279143a 56 void dW()
mazdo25 3:01b5e80d842d 57 {
mazdo25 4:208f5279143a 58 RoboticAngularVelocity = leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity();
mazdo25 4:208f5279143a 59 }
mazdo25 4:208f5279143a 60
mazdo25 4:208f5279143a 61 float returnRoboticAngularVelocity()
mazdo25 4:208f5279143a 62 {
mazdo25 4:208f5279143a 63 return RoboticAngularVelocity;
mazdo25 3:01b5e80d842d 64 }
mazdo25 3:01b5e80d842d 65
mazdo25 3:01b5e80d842d 66 //attempts to modify the angular velocity of the buggy
mazdo25 3:01b5e80d842d 67 void adjustAngularVelocity(float W)
mazdo25 3:01b5e80d842d 68 {
mazdo25 4:208f5279143a 69 //negative is a right turn
mazdo25 3:01b5e80d842d 70 if (W < 0)
mazdo25 3:01b5e80d842d 71 {
mazdo25 5:f1613df66ceb 72 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
mazdo25 5:f1613df66ceb 73 leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()+W)*AF);
mazdo25 3:01b5e80d842d 74 }
mazdo25 3:01b5e80d842d 75 else
mazdo25 3:01b5e80d842d 76 {
mazdo25 4:208f5279143a 77 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
mazdo25 5:f1613df66ceb 78 rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()-W)*AF);
mazdo25 4:208f5279143a 79 }
mazdo25 3:01b5e80d842d 80 }
mazdo25 3:01b5e80d842d 81
mazdo25 3:01b5e80d842d 82 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 83 {
mazdo25 7:cb07cdb35b6c 84
mazdo25 7:cb07cdb35b6c 85 float ambientLight;
mazdo25 7:cb07cdb35b6c 86 float reading;
mazdo25 7:cb07cdb35b6c 87 ambientLight = sensorArray[sensorNumber]->calcLineVoltage();
mazdo25 7:cb07cdb35b6c 88 sensorArray[sensorNumber]->sample();
mazdo25 7:cb07cdb35b6c 89 sensorArray[sensorNumber]->calcLineVoltage();
mazdo25 7:cb07cdb35b6c 90 switch (sensorNumber)
mazdo25 7:cb07cdb35b6c 91 {
mazdo25 4:208f5279143a 92 case 0:
mazdo25 5:f1613df66ceb 93 lineVoltages[lvIndex%numberOfSamples] = ((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -400.0f);
mazdo25 4:208f5279143a 94 break;
mazdo25 4:208f5279143a 95 case 1:
mazdo25 5:f1613df66ceb 96 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -200.0f);
mazdo25 4:208f5279143a 97 break;
mazdo25 4:208f5279143a 98 case 2:
mazdo25 5:f1613df66ceb 99 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -100.0f);
mazdo25 4:208f5279143a 100 break;
mazdo25 4:208f5279143a 101 case 3:
mazdo25 5:f1613df66ceb 102 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 100.0f);
mazdo25 4:208f5279143a 103 break;
mazdo25 5:f1613df66ceb 104 case 4:
mazdo25 5:f1613df66ceb 105 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 200.0f);
mazdo25 4:208f5279143a 106 break;
mazdo25 4:208f5279143a 107 case 5:
mazdo25 5:f1613df66ceb 108 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 400.0f);
mazdo25 4:208f5279143a 109 break;
mazdo25 7:cb07cdb35b6c 110 }
mazdo25 7:cb07cdb35b6c 111 sensorNumber ++;
mazdo25 7:cb07cdb35b6c 112 if (reading <= Reflec*11) {endOfLineDetection++;}
mazdo25 7:cb07cdb35b6c 113
mazdo25 7:cb07cdb35b6c 114 if (sensorNumber >= numberOfSensors)
mazdo25 7:cb07cdb35b6c 115 {
mazdo25 7:cb07cdb35b6c 116 if (endOfLineDetection <= 6)
mazdo25 7:cb07cdb35b6c 117 {
mazdo25 7:cb07cdb35b6c 118 AF = 0.3f;
mazdo25 7:cb07cdb35b6c 119 sensorNumber = 0;
mazdo25 7:cb07cdb35b6c 120 adjustAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
mazdo25 7:cb07cdb35b6c 121 lvIndex++;
mazdo25 7:cb07cdb35b6c 122 }
mazdo25 7:cb07cdb35b6c 123 else
mazdo25 7:cb07cdb35b6c 124 {
mazdo25 7:cb07cdb35b6c 125 if (AF > 0) //this will Slowly grind the buggy to a HALT to overcome line breaks and overshooting in turns -> dont want it to instantly stop
mazdo25 7:cb07cdb35b6c 126 {
mazdo25 7:cb07cdb35b6c 127 AF -= 0.1f;
mazdo25 7:cb07cdb35b6c 128 adjustAngularVelocity(lineVoltages[lvIndex%numberOfSamples-1]); //base your action on the previous measured value
mazdo25 7:cb07cdb35b6c 129 }
mazdo25 7:cb07cdb35b6c 130 else //well shit, you're buggy must have totally overshot the line, lets just hope while spinning it can find a line, direction not guaranteed
mazdo25 7:cb07cdb35b6c 131 {
mazdo25 7:cb07cdb35b6c 132 spin(); //spin like mad and hope you find a line
mazdo25 7:cb07cdb35b6c 133 }
mazdo25 7:cb07cdb35b6c 134 }
mazdo25 7:cb07cdb35b6c 135 }
mazdo25 3:01b5e80d842d 136 }
mazdo25 3:01b5e80d842d 137
mazdo25 3:01b5e80d842d 138 void stopMovement(void)
mazdo25 3:01b5e80d842d 139 {
mazdo25 3:01b5e80d842d 140 leftWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 141 rightWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 142 }
mazdo25 5:f1613df66ceb 143
mazdo25 5:f1613df66ceb 144 float returnLineVoltage()
mazdo25 5:f1613df66ceb 145 {
mazdo25 5:f1613df66ceb 146 return lineVoltages[lvIndex%numberOfSamples];
mazdo25 5:f1613df66ceb 147 }
mazdo25 5:f1613df66ceb 148
mazdo25 5:f1613df66ceb 149 void rbtInit()
mazdo25 5:f1613df66ceb 150 {
mazdo25 6:477382219bcf 151 sensorArray[0]->sample();
mazdo25 6:477382219bcf 152 Reflec = sensorArray[0]->calcLineVoltage();
mazdo25 6:477382219bcf 153 controller.setInputLimits(-300.0f,300.0f);
mazdo25 6:477382219bcf 154 controller.setSetPoint(0.0f);
mazdo25 6:477382219bcf 155 updater.attach(callback(this, &Robot::robotUpdates),0.0001f);
mazdo25 5:f1613df66ceb 156 }
mazdo25 7:cb07cdb35b6c 157
mazdo25 7:cb07cdb35b6c 158 void spin()
mazdo25 7:cb07cdb35b6c 159 {
mazdo25 7:cb07cdb35b6c 160 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.3f);
mazdo25 7:cb07cdb35b6c 161 leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel())*-0.3f);
mazdo25 7:cb07cdb35b6c 162 }
mazdo25 7:cb07cdb35b6c 163
mazdo25 7:cb07cdb35b6c 164 void setState(char S)
mazdo25 7:cb07cdb35b6c 165 {
mazdo25 7:cb07cdb35b6c 166 state = S;
mazdo25 7:cb07cdb35b6c 167 }
mazdo25 3:01b5e80d842d 168
mazdo25 3:01b5e80d842d 169 };
mazdo25 3:01b5e80d842d 170
mazdo25 3:01b5e80d842d 171 /*
mazdo25 3:01b5e80d842d 172 Timeout timeToStop,
mazdo25 3:01b5e80d842d 173 void turn(float degrees) //+ive -> right Turn -ive -> leftTurn
mazdo25 3:01b5e80d842d 174 {
mazdo25 3:01b5e80d842d 175 float ratioOfTurn = ((abs((int)degrees) % 360)/360); //returns a value 0 -> 0.9999999 which is how much turning it has to do
mazdo25 3:01b5e80d842d 176 if (ratioOfTurn == 0.0f) {return;}
mazdo25 3:01b5e80d842d 177 rightWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 178 leftWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 179 wait(0.1);
mazdo25 3:01b5e80d842d 180 if (degrees > 0 )
mazdo25 3:01b5e80d842d 181 {
mazdo25 3:01b5e80d842d 182 rightWheel->adjustAngularVelocity(rightWheel->maxAngularVel);
mazdo25 3:01b5e80d842d 183 leftWheel->adjustAngularVelocity((leftWheel->maxAngularVel)*-1.0f);
mazdo25 3:01b5e80d842d 184 }
mazdo25 3:01b5e80d842d 185 else
mazdo25 3:01b5e80d842d 186 {
mazdo25 3:01b5e80d842d 187 rightWheel->adjustAngularVelocity((rightWheel->maxAngularVel)*-1.0f);
mazdo25 3:01b5e80d842d 188 leftWheel->adjustAngularVelocity(leftWheel->maxAngularVel);
mazdo25 3:01b5e80d842d 189 }
mazdo25 3:01b5e80d842d 190
mazdo25 3:01b5e80d842d 191 timeToStop.attach(callback(this, &Robot::stopMovement),ratioOfTurn*((distanceBetweenWheels*(float)PI)/(((rightWheel->maxAngularVel)*-1.0f) * 256.0f * 2.0f * (float)PI * Wheel::wheelDiameter)));
mazdo25 3:01b5e80d842d 192 }
mazdo25 3:01b5e80d842d 193
mazdo25 3:01b5e80d842d 194 void travelDistance(float d)//in metres
mazdo25 3:01b5e80d842d 195 {
mazdo25 3:01b5e80d842d 196 timeToStop.attach(callback(this, &Robot::stopMovement), d/(calculateTranslationalVelocity()*(float)PI*Wheel::wheelDiameter));
mazdo25 3:01b5e80d842d 197 }
mazdo25 3:01b5e80d842d 198
mazdo25 3:01b5e80d842d 199 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 200 {
mazdo25 3:01b5e80d842d 201 sensorArray[sensorNumber]->sample();
mazdo25 3:01b5e80d842d 202
mazdo25 3:01b5e80d842d 203 if (sensorNumber < (numberOfSensors/2))
mazdo25 3:01b5e80d842d 204 {
mazdo25 3:01b5e80d842d 205 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-3);
mazdo25 3:01b5e80d842d 206 }
mazdo25 3:01b5e80d842d 207 else
mazdo25 3:01b5e80d842d 208 }
mazdo25 3:01b5e80d842d 209 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-2);
mazdo25 3:01b5e80d842d 210 }
mazdo25 3:01b5e80d842d 211
mazdo25 3:01b5e80d842d 212 sensorNumber++;
mazdo25 3:01b5e80d842d 213 if (sensorNumber % numberOfSensors == 0)
mazdo25 3:01b5e80d842d 214 {
mazdo25 3:01b5e80d842d 215 sensorNumber = 0;
mazdo25 3:01b5e80d842d 216 controller.setProcessValue(lineVoltages[(lvIndex%numberOfSamples)];
mazdo25 3:01b5e80d842d 217 adjustAngularVelocity(controller.compute());
mazdo25 3:01b5e80d842d 218 lvIndex++;
mazdo25 3:01b5e80d842d 219 }
mazdo25 3:01b5e80d842d 220 }
mazdo25 3:01b5e80d842d 221
mazdo25 3:01b5e80d842d 222 */
mazdo25 4:208f5279143a 223 //lineVoltages[lvIndex] += 0.5f;
mazdo25 4:208f5279143a 224
mazdo25 4:208f5279143a 225
mazdo25 4:208f5279143a 226
mazdo25 4:208f5279143a 227 /*if input value is greater than the maximum value the left wheel can go, go full right TURN
mazdo25 4:208f5279143a 228 if (W > leftWheel->returnMaxAngularVel()*AF)
mazdo25 4:208f5279143a 229 {
mazdo25 4:208f5279143a 230 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 231 rightWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 232 }
mazdo25 4:208f5279143a 233 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 234 {
mazdo25 4:208f5279143a 235 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 236 leftWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 237 }
mazdo25 4:208f5279143a 238 else if (W == 0)
mazdo25 4:208f5279143a 239 {
mazdo25 4:208f5279143a 240 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 241 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 242 }
mazdo25 4:208f5279143a 243 else
mazdo25 4:208f5279143a 244 {
mazdo25 4:208f5279143a 245
mazdo25 4:208f5279143a 246 */