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@6:477382219bcf, 2019-03-26 (annotated)

Committer:
mazdo25
Date:
Tue Mar 26 16:19:47 2019 +0000
Revision:
6:477382219bcf
Parent:
5:f1613df66ceb
Child:
7:cb07cdb35b6c
LATEST WORKING

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mazdo25 3:01b5e80d842d 1 class Robot {
mazdo25 3:01b5e80d842d 2 private:
mazdo25 3:01b5e80d842d 3 float static const distanceBetweenWheels = 0.19;
mazdo25 3:01b5e80d842d 4 int static const numberOfSamples = 100;
mazdo25 3:01b5e80d842d 5 int static const numberOfSensors = 6;
mazdo25 4:208f5279143a 6 //attenuation factor
mazdo25 6:477382219bcf 7 float static const AF = 0.3f;
mazdo25 5:f1613df66ceb 8 Wheel* leftWheel;
mazdo25 5:f1613df66ceb 9 Wheel* rightWheel;
mazdo25 3:01b5e80d842d 10 lineSensor* sensorArray[numberOfSensors];
mazdo25 5:f1613df66ceb 11 PID2 controller;
mazdo25 3:01b5e80d842d 12 float lineVoltages[numberOfSamples];
mazdo25 3:01b5e80d842d 13 int lvIndex;
mazdo25 3:01b5e80d842d 14 int sensorNumber;
mazdo25 6:477382219bcf 15
mazdo25 3:01b5e80d842d 16 Ticker updater;
mazdo25 6:477382219bcf 17
mazdo25 5:f1613df66ceb 18 int endOfLineDetection;
mazdo25 6:477382219bcf 19 float Reflec;
mazdo25 3:01b5e80d842d 20
mazdo25 4:208f5279143a 21 float RoboticAngularVelocity;
mazdo25 4:208f5279143a 22
mazdo25 3:01b5e80d842d 23 public:
mazdo25 3:01b5e80d842d 24
mazdo25 6:477382219bcf 25 Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(2.0f, 0.0f, 0.0f, 0.0006f)
mazdo25 3:01b5e80d842d 26 {
mazdo25 6:477382219bcf 27 updater.detach();
mazdo25 6:477382219bcf 28
mazdo25 6:477382219bcf 29 Reflec = 0;
mazdo25 5:f1613df66ceb 30 lvIndex = 0;
mazdo25 4:208f5279143a 31 sensorNumber = 0;
mazdo25 3:01b5e80d842d 32 sensorArray[0] = SA[0];
mazdo25 4:208f5279143a 33 sensorArray[1] = SA[1];
mazdo25 4:208f5279143a 34 sensorArray[2] = SA[2];
mazdo25 4:208f5279143a 35 sensorArray[3] = SA[3];
mazdo25 4:208f5279143a 36 sensorArray[4] = SA[4];
mazdo25 4:208f5279143a 37 sensorArray[5] = SA[5];
mazdo25 3:01b5e80d842d 38 leftWheel = LW;
mazdo25 3:01b5e80d842d 39 rightWheel= RW;
mazdo25 3:01b5e80d842d 40
mazdo25 4:208f5279143a 41 lineVoltages[lvIndex] = 0.0f;
mazdo25 3:01b5e80d842d 42 //controller will output a value between +- max speed of wheels
mazdo25 5:f1613df66ceb 43
mazdo25 5:f1613df66ceb 44 controller.assignLimitAddress(rightWheel->returnMinAVptr(),leftWheel->returnMaxAVptr());
mazdo25 3:01b5e80d842d 45 };
mazdo25 3:01b5e80d842d 46
mazdo25 3:01b5e80d842d 47 float calculateTranslationalVelocity()
mazdo25 3:01b5e80d842d 48 {
mazdo25 3:01b5e80d842d 49 return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
mazdo25 3:01b5e80d842d 50 }
mazdo25 4:208f5279143a 51
mazdo25 4:208f5279143a 52 //difference between angular velocities.
mazdo25 4:208f5279143a 53 void dW()
mazdo25 3:01b5e80d842d 54 {
mazdo25 4:208f5279143a 55 RoboticAngularVelocity = leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity();
mazdo25 4:208f5279143a 56 }
mazdo25 4:208f5279143a 57
mazdo25 4:208f5279143a 58 float returnRoboticAngularVelocity()
mazdo25 4:208f5279143a 59 {
mazdo25 4:208f5279143a 60 return RoboticAngularVelocity;
mazdo25 3:01b5e80d842d 61 }
mazdo25 3:01b5e80d842d 62
mazdo25 3:01b5e80d842d 63 //attempts to modify the angular velocity of the buggy
mazdo25 3:01b5e80d842d 64 void adjustAngularVelocity(float W)
mazdo25 3:01b5e80d842d 65 {
mazdo25 4:208f5279143a 66 //negative is a right turn
mazdo25 3:01b5e80d842d 67 if (W < 0)
mazdo25 3:01b5e80d842d 68 {
mazdo25 5:f1613df66ceb 69 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
mazdo25 5:f1613df66ceb 70 leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()+W)*AF);
mazdo25 3:01b5e80d842d 71 }
mazdo25 3:01b5e80d842d 72 else
mazdo25 3:01b5e80d842d 73 {
mazdo25 4:208f5279143a 74 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
mazdo25 5:f1613df66ceb 75 rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()-W)*AF);
mazdo25 4:208f5279143a 76 }
mazdo25 3:01b5e80d842d 77 }
mazdo25 3:01b5e80d842d 78
mazdo25 3:01b5e80d842d 79 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 80 {
mazdo25 4:208f5279143a 81 float ambientLight;
mazdo25 6:477382219bcf 82 float reading;
mazdo25 5:f1613df66ceb 83 ambientLight = sensorArray[sensorNumber]->calcLineVoltage();
mazdo25 5:f1613df66ceb 84 sensorArray[sensorNumber]->sample();
mazdo25 4:208f5279143a 85 sensorArray[sensorNumber]->calcLineVoltage();
mazdo25 4:208f5279143a 86 switch (sensorNumber)
mazdo25 4:208f5279143a 87 {
mazdo25 4:208f5279143a 88 case 0:
mazdo25 5:f1613df66ceb 89 lineVoltages[lvIndex%numberOfSamples] = ((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -400.0f);
mazdo25 4:208f5279143a 90 break;
mazdo25 4:208f5279143a 91 case 1:
mazdo25 5:f1613df66ceb 92 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -200.0f);
mazdo25 4:208f5279143a 93 break;
mazdo25 4:208f5279143a 94 case 2:
mazdo25 5:f1613df66ceb 95 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -100.0f);
mazdo25 4:208f5279143a 96 break;
mazdo25 4:208f5279143a 97 case 3:
mazdo25 5:f1613df66ceb 98 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 100.0f);
mazdo25 4:208f5279143a 99 break;
mazdo25 5:f1613df66ceb 100 case 4:
mazdo25 5:f1613df66ceb 101 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 200.0f);
mazdo25 4:208f5279143a 102 break;
mazdo25 4:208f5279143a 103 case 5:
mazdo25 5:f1613df66ceb 104 lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 400.0f);
mazdo25 4:208f5279143a 105 break;
mazdo25 4:208f5279143a 106 }
mazdo25 4:208f5279143a 107 sensorNumber ++;
mazdo25 5:f1613df66ceb 108 if (sensorNumber >= numberOfSensors)
mazdo25 4:208f5279143a 109 {
mazdo25 5:f1613df66ceb 110 sensorNumber = 0;
mazdo25 5:f1613df66ceb 111 adjustAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
mazdo25 4:208f5279143a 112 lvIndex++;
mazdo25 4:208f5279143a 113 }
mazdo25 3:01b5e80d842d 114 }
mazdo25 3:01b5e80d842d 115
mazdo25 3:01b5e80d842d 116 void stopMovement(void)
mazdo25 3:01b5e80d842d 117 {
mazdo25 3:01b5e80d842d 118 leftWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 119 rightWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 120 }
mazdo25 5:f1613df66ceb 121
mazdo25 5:f1613df66ceb 122 float returnLineVoltage()
mazdo25 5:f1613df66ceb 123 {
mazdo25 5:f1613df66ceb 124 return lineVoltages[lvIndex%numberOfSamples];
mazdo25 5:f1613df66ceb 125 }
mazdo25 5:f1613df66ceb 126
mazdo25 5:f1613df66ceb 127 void rbtInit()
mazdo25 5:f1613df66ceb 128 {
mazdo25 6:477382219bcf 129 sensorArray[0]->sample();
mazdo25 6:477382219bcf 130 Reflec = sensorArray[0]->calcLineVoltage();
mazdo25 6:477382219bcf 131 controller.setInputLimits(-300.0f,300.0f);
mazdo25 6:477382219bcf 132 controller.setSetPoint(0.0f);
mazdo25 6:477382219bcf 133 updater.attach(callback(this, &Robot::robotUpdates),0.0001f);
mazdo25 5:f1613df66ceb 134 }
mazdo25 3:01b5e80d842d 135
mazdo25 3:01b5e80d842d 136 };
mazdo25 3:01b5e80d842d 137
mazdo25 3:01b5e80d842d 138 /*
mazdo25 3:01b5e80d842d 139 Timeout timeToStop,
mazdo25 3:01b5e80d842d 140 void turn(float degrees) //+ive -> right Turn -ive -> leftTurn
mazdo25 3:01b5e80d842d 141 {
mazdo25 3:01b5e80d842d 142 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 143 if (ratioOfTurn == 0.0f) {return;}
mazdo25 3:01b5e80d842d 144 rightWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 145 leftWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 146 wait(0.1);
mazdo25 3:01b5e80d842d 147 if (degrees > 0 )
mazdo25 3:01b5e80d842d 148 {
mazdo25 3:01b5e80d842d 149 rightWheel->adjustAngularVelocity(rightWheel->maxAngularVel);
mazdo25 3:01b5e80d842d 150 leftWheel->adjustAngularVelocity((leftWheel->maxAngularVel)*-1.0f);
mazdo25 3:01b5e80d842d 151 }
mazdo25 3:01b5e80d842d 152 else
mazdo25 3:01b5e80d842d 153 {
mazdo25 3:01b5e80d842d 154 rightWheel->adjustAngularVelocity((rightWheel->maxAngularVel)*-1.0f);
mazdo25 3:01b5e80d842d 155 leftWheel->adjustAngularVelocity(leftWheel->maxAngularVel);
mazdo25 3:01b5e80d842d 156 }
mazdo25 3:01b5e80d842d 157
mazdo25 3:01b5e80d842d 158 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 159 }
mazdo25 3:01b5e80d842d 160
mazdo25 3:01b5e80d842d 161 void travelDistance(float d)//in metres
mazdo25 3:01b5e80d842d 162 {
mazdo25 3:01b5e80d842d 163 timeToStop.attach(callback(this, &Robot::stopMovement), d/(calculateTranslationalVelocity()*(float)PI*Wheel::wheelDiameter));
mazdo25 3:01b5e80d842d 164 }
mazdo25 3:01b5e80d842d 165
mazdo25 3:01b5e80d842d 166 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 167 {
mazdo25 3:01b5e80d842d 168 sensorArray[sensorNumber]->sample();
mazdo25 3:01b5e80d842d 169
mazdo25 3:01b5e80d842d 170 if (sensorNumber < (numberOfSensors/2))
mazdo25 3:01b5e80d842d 171 {
mazdo25 3:01b5e80d842d 172 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-3);
mazdo25 3:01b5e80d842d 173 }
mazdo25 3:01b5e80d842d 174 else
mazdo25 3:01b5e80d842d 175 }
mazdo25 3:01b5e80d842d 176 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-2);
mazdo25 3:01b5e80d842d 177 }
mazdo25 3:01b5e80d842d 178
mazdo25 3:01b5e80d842d 179 sensorNumber++;
mazdo25 3:01b5e80d842d 180 if (sensorNumber % numberOfSensors == 0)
mazdo25 3:01b5e80d842d 181 {
mazdo25 3:01b5e80d842d 182 sensorNumber = 0;
mazdo25 3:01b5e80d842d 183 controller.setProcessValue(lineVoltages[(lvIndex%numberOfSamples)];
mazdo25 3:01b5e80d842d 184 adjustAngularVelocity(controller.compute());
mazdo25 3:01b5e80d842d 185 lvIndex++;
mazdo25 3:01b5e80d842d 186 }
mazdo25 3:01b5e80d842d 187 }
mazdo25 3:01b5e80d842d 188
mazdo25 3:01b5e80d842d 189 */
mazdo25 4:208f5279143a 190 //lineVoltages[lvIndex] += 0.5f;
mazdo25 4:208f5279143a 191
mazdo25 4:208f5279143a 192
mazdo25 4:208f5279143a 193
mazdo25 4:208f5279143a 194 /*if input value is greater than the maximum value the left wheel can go, go full right TURN
mazdo25 4:208f5279143a 195 if (W > leftWheel->returnMaxAngularVel()*AF)
mazdo25 4:208f5279143a 196 {
mazdo25 4:208f5279143a 197 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 198 rightWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 199 }
mazdo25 4:208f5279143a 200 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 201 {
mazdo25 4:208f5279143a 202 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 203 leftWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 204 }
mazdo25 4:208f5279143a 205 else if (W == 0)
mazdo25 4:208f5279143a 206 {
mazdo25 4:208f5279143a 207 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 208 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 209 }
mazdo25 4:208f5279143a 210 else
mazdo25 4:208f5279143a 211 {
mazdo25 4:208f5279143a 212
mazdo25 4:208f5279143a 213 */