Buggy bois / Mbed 2 deprecated WORKING_TRIAL

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@5:f1613df66ceb, 2019-03-25 (annotated)

Committer:
mazdo25
Date:
Mon Mar 25 22:42:31 2019 +0000
Revision:
5:f1613df66ceb
Parent:
4:208f5279143a
Child:
6:477382219bcf
LATEST WORKING VERSION;

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