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@8:5ed6685f6edd, 2019-03-31 (annotated)

Committer:
mazdo25
Date:
Sun Mar 31 20:31:30 2019 +0000
Revision:
8:5ed6685f6edd
Parent:
7:cb07cdb35b6c
Child:
9:cefa177c1353
version used in test demo 3

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 7:cb07cdb35b6c 27 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 28 {
mazdo25 6:477382219bcf 29 updater.detach();
mazdo25 8:5ed6685f6edd 30 timeToStop.detach();
mazdo25 7:cb07cdb35b6c 31 AF = 0.3f; //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 8:5ed6685f6edd 48 controller.setInputLimits(-300.0f,300.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 8:5ed6685f6edd 144 lineVoltages[lvIndex%numberOfSamples] = (reading * -100.0f);
mazdo25 8:5ed6685f6edd 145 if (reading <= Reflec*10){endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 146 break;
mazdo25 8:5ed6685f6edd 147 case 1:
mazdo25 8:5ed6685f6edd 148 lineVoltages[lvIndex%numberOfSamples] += (reading * -50.0f);
mazdo25 8:5ed6685f6edd 149 if (reading <= Reflec*10) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 150 break;
mazdo25 8:5ed6685f6edd 151 case 2:
mazdo25 8:5ed6685f6edd 152 lineVoltages[lvIndex%numberOfSamples] += (reading * -25.0f);
mazdo25 8:5ed6685f6edd 153 if (reading <= 0.5f) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 154 break;
mazdo25 8:5ed6685f6edd 155 case 3:
mazdo25 8:5ed6685f6edd 156 lineVoltages[lvIndex%numberOfSamples] += (reading * 25.0f);
mazdo25 8:5ed6685f6edd 157 if (reading <= 0.5f) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 158 break;
mazdo25 8:5ed6685f6edd 159 case 4:
mazdo25 8:5ed6685f6edd 160 lineVoltages[lvIndex%numberOfSamples] += (reading * 50.0f);
mazdo25 8:5ed6685f6edd 161 if (reading <= Reflec*10) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 162 break;
mazdo25 8:5ed6685f6edd 163 case 5:
mazdo25 8:5ed6685f6edd 164 lineVoltages[lvIndex%numberOfSamples] += (reading * 100.0f);
mazdo25 8:5ed6685f6edd 165 if (reading <= Reflec*10) {endOfLineDetection++;}
mazdo25 8:5ed6685f6edd 166 break;
mazdo25 8:5ed6685f6edd 167 }
mazdo25 8:5ed6685f6edd 168
mazdo25 8:5ed6685f6edd 169 sensorNumber ++;
mazdo25 8:5ed6685f6edd 170
mazdo25 8:5ed6685f6edd 171 if (sensorNumber >= numberOfSensors)
mazdo25 8:5ed6685f6edd 172 {
mazdo25 8:5ed6685f6edd 173 sensorNumber = 0;
mazdo25 8:5ed6685f6edd 174 if (endOfLineDetection < 6)
mazdo25 8:5ed6685f6edd 175 {
mazdo25 8:5ed6685f6edd 176 AF = 0.3f;
mazdo25 8:5ed6685f6edd 177 adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
mazdo25 8:5ed6685f6edd 178 lvIndex++;
mazdo25 8:5ed6685f6edd 179 }
mazdo25 8:5ed6685f6edd 180 else if (AF > 0.1f)
mazdo25 8:5ed6685f6edd 181 {
mazdo25 8:5ed6685f6edd 182 AF -= 0.1f;
mazdo25 8:5ed6685f6edd 183 adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples]-1);
mazdo25 8:5ed6685f6edd 184 }else{
mazdo25 8:5ed6685f6edd 185 AF = 0.0f;
mazdo25 8:5ed6685f6edd 186 stopMovement();
mazdo25 8:5ed6685f6edd 187 }
mazdo25 8:5ed6685f6edd 188 endOfLineDetection = 0;
mazdo25 8:5ed6685f6edd 189 }
mazdo25 8:5ed6685f6edd 190 }
mazdo25 8:5ed6685f6edd 191
mazdo25 8:5ed6685f6edd 192 void turn180()
mazdo25 8:5ed6685f6edd 193 {
mazdo25 8:5ed6685f6edd 194 updater.detach();
mazdo25 8:5ed6685f6edd 195 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.3f);
mazdo25 8:5ed6685f6edd 196 leftWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*-0.3f);
mazdo25 8:5ed6685f6edd 197 timeToStop.attach(callback(this, &Robot::reAttach),1.3f);
mazdo25 8:5ed6685f6edd 198 state = 'S';
mazdo25 8:5ed6685f6edd 199 }
mazdo25 8:5ed6685f6edd 200
mazdo25 8:5ed6685f6edd 201 void reAttach()
mazdo25 8:5ed6685f6edd 202 {
mazdo25 8:5ed6685f6edd 203 updater.attach(callback(this, &Robot::robotUpdates),0.0001f);
mazdo25 8:5ed6685f6edd 204 }
mazdo25 3:01b5e80d842d 205
mazdo25 3:01b5e80d842d 206 };
mazdo25 3:01b5e80d842d 207
mazdo25 3:01b5e80d842d 208 /*
mazdo25 3:01b5e80d842d 209 Timeout timeToStop,
mazdo25 8:5ed6685f6edd 210
mazdo25 3:01b5e80d842d 211
mazdo25 3:01b5e80d842d 212 void travelDistance(float d)//in metres
mazdo25 3:01b5e80d842d 213 {
mazdo25 3:01b5e80d842d 214 timeToStop.attach(callback(this, &Robot::stopMovement), d/(calculateTranslationalVelocity()*(float)PI*Wheel::wheelDiameter));
mazdo25 3:01b5e80d842d 215 }
mazdo25 3:01b5e80d842d 216
mazdo25 3:01b5e80d842d 217 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 218 {
mazdo25 3:01b5e80d842d 219 sensorArray[sensorNumber]->sample();
mazdo25 3:01b5e80d842d 220
mazdo25 3:01b5e80d842d 221 if (sensorNumber < (numberOfSensors/2))
mazdo25 3:01b5e80d842d 222 {
mazdo25 3:01b5e80d842d 223 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-3);
mazdo25 3:01b5e80d842d 224 }
mazdo25 3:01b5e80d842d 225 else
mazdo25 3:01b5e80d842d 226 }
mazdo25 3:01b5e80d842d 227 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-2);
mazdo25 3:01b5e80d842d 228 }
mazdo25 3:01b5e80d842d 229
mazdo25 3:01b5e80d842d 230 sensorNumber++;
mazdo25 3:01b5e80d842d 231 if (sensorNumber % numberOfSensors == 0)
mazdo25 3:01b5e80d842d 232 {
mazdo25 3:01b5e80d842d 233 sensorNumber = 0;
mazdo25 3:01b5e80d842d 234 controller.setProcessValue(lineVoltages[(lvIndex%numberOfSamples)];
mazdo25 3:01b5e80d842d 235 adjustAngularVelocity(controller.compute());
mazdo25 3:01b5e80d842d 236 lvIndex++;
mazdo25 3:01b5e80d842d 237 }
mazdo25 3:01b5e80d842d 238 }
mazdo25 3:01b5e80d842d 239
mazdo25 3:01b5e80d842d 240 */
mazdo25 4:208f5279143a 241 //lineVoltages[lvIndex] += 0.5f;
mazdo25 4:208f5279143a 242
mazdo25 4:208f5279143a 243
mazdo25 4:208f5279143a 244
mazdo25 4:208f5279143a 245 /*if input value is greater than the maximum value the left wheel can go, go full right TURN
mazdo25 4:208f5279143a 246 if (W > leftWheel->returnMaxAngularVel()*AF)
mazdo25 4:208f5279143a 247 {
mazdo25 4:208f5279143a 248 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 249 rightWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 250 }
mazdo25 4:208f5279143a 251 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 252 {
mazdo25 4:208f5279143a 253 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 254 leftWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 255 }
mazdo25 4:208f5279143a 256 else if (W == 0)
mazdo25 4:208f5279143a 257 {
mazdo25 4:208f5279143a 258 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 259 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 260 }
mazdo25 4:208f5279143a 261 else
mazdo25 4:208f5279143a 262 {
mazdo25 4:208f5279143a 263
mazdo25 4:208f5279143a 264 */