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@4:208f5279143a, 2019-03-23 (annotated)

Committer:
mazdo25
Date:
Sat Mar 23 19:46:09 2019 +0000
Revision:
4:208f5279143a
Parent:
3:01b5e80d842d
Child:
5:f1613df66ceb
latest working, but line following not;

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 4:208f5279143a 7 float static const AF = 1.0f;
mazdo25 3:01b5e80d842d 8
mazdo25 4:208f5279143a 9 LWheel* leftWheel;
mazdo25 4:208f5279143a 10 RWheel* rightWheel;
mazdo25 3:01b5e80d842d 11 lineSensor* sensorArray[numberOfSensors];
mazdo25 3:01b5e80d842d 12 PID controller;
mazdo25 3:01b5e80d842d 13 float lineVoltages[numberOfSamples];
mazdo25 3:01b5e80d842d 14 int lvIndex;
mazdo25 3:01b5e80d842d 15 int sensorNumber;
mazdo25 3:01b5e80d842d 16 Ticker updater;
mazdo25 3:01b5e80d842d 17
mazdo25 4:208f5279143a 18 float RoboticAngularVelocity;
mazdo25 4:208f5279143a 19
mazdo25 3:01b5e80d842d 20 public:
mazdo25 3:01b5e80d842d 21
mazdo25 4:208f5279143a 22 Robot(LWheel* LW, RWheel* RW, lineSensor* SA[]) : controller(1.0f,0.0f,0.0f,0.1f)
mazdo25 3:01b5e80d842d 23 {
mazdo25 3:01b5e80d842d 24 lvIndex = 1;
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 4:208f5279143a 37 controller.setOutputLimits(-1.0f*AF*rightWheel->returnMaxAngularVel(),AF*(leftWheel->returnMaxAngularVel()));
mazdo25 4:208f5279143a 38 controller.setInputLimits(-1980.0f,1980.0f);
mazdo25 4:208f5279143a 39 controller.setSetPoint(0.0f);
mazdo25 4:208f5279143a 40 updater.attach(callback(this, &Robot::robotUpdates),0.1f);
mazdo25 3:01b5e80d842d 41 };
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 float temp = W - RoboticAngularVelocity;
mazdo25 4:208f5279143a 64 //negative is a right turn
mazdo25 3:01b5e80d842d 65 if (W < 0)
mazdo25 3:01b5e80d842d 66 {
mazdo25 4:208f5279143a 67 //leftWheel a smaller number depending on difference
mazdo25 4:208f5279143a 68 leftWheel->adjustAngularVelocity((leftWheel->returnAngularVelocity()+(temp+(rightWheel->returnMaxAngularVel()-rightWheel->returnAngularVelocity()))*AF));
mazdo25 4:208f5279143a 69 //right Wheel on maximum
mazdo25 4:208f5279143a 70 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
mazdo25 3:01b5e80d842d 71 }
mazdo25 3:01b5e80d842d 72 else
mazdo25 3:01b5e80d842d 73 {
mazdo25 4:208f5279143a 74 rightWheel->adjustAngularVelocity((rightWheel->returnAngularVelocity()-(temp-(leftWheel->returnMaxAngularVel()-leftWheel->returnAngularVelocity()))*AF));
mazdo25 4:208f5279143a 75 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*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 4:208f5279143a 82 sensorArray[sensorNumber]->calcLineVoltage();
mazdo25 4:208f5279143a 83 ambientLight = sensorArray[sensorNumber]->returnLineVoltage();
mazdo25 4:208f5279143a 84 sensorArray[sensorNumber]->sample();
mazdo25 4:208f5279143a 85 switch (sensorNumber)
mazdo25 4:208f5279143a 86 {
mazdo25 4:208f5279143a 87 case 0:
mazdo25 4:208f5279143a 88 lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -400.0f;
mazdo25 4:208f5279143a 89 break;
mazdo25 4:208f5279143a 90 case 1:
mazdo25 4:208f5279143a 91 lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -200.0f;
mazdo25 4:208f5279143a 92 break;
mazdo25 4:208f5279143a 93 case 2:
mazdo25 4:208f5279143a 94 lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -100.0f;
mazdo25 4:208f5279143a 95 break;
mazdo25 4:208f5279143a 96 case 3:
mazdo25 4:208f5279143a 97 lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 100.0f;
mazdo25 4:208f5279143a 98 break;
mazdo25 4:208f5279143a 99 case 4:
mazdo25 4:208f5279143a 100 lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 200.0f;
mazdo25 4:208f5279143a 101 break;
mazdo25 4:208f5279143a 102 case 5:
mazdo25 4:208f5279143a 103 lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 400.0f;
mazdo25 4:208f5279143a 104 break;
mazdo25 4:208f5279143a 105 }
mazdo25 4:208f5279143a 106 sensorNumber ++;
mazdo25 4:208f5279143a 107 if (sensorNumber >= 4)
mazdo25 4:208f5279143a 108 {
mazdo25 4:208f5279143a 109 sensorNumber = 1;
mazdo25 4:208f5279143a 110 adjustAngularVelocity(controller.compute(lineVoltages[lvIndex]));
mazdo25 4:208f5279143a 111 lvIndex++;
mazdo25 4:208f5279143a 112 }
mazdo25 3:01b5e80d842d 113 }
mazdo25 3:01b5e80d842d 114
mazdo25 3:01b5e80d842d 115 void stopMovement(void)
mazdo25 3:01b5e80d842d 116 {
mazdo25 3:01b5e80d842d 117 leftWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 118 rightWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 119 }
mazdo25 3:01b5e80d842d 120
mazdo25 3:01b5e80d842d 121 };
mazdo25 3:01b5e80d842d 122
mazdo25 3:01b5e80d842d 123 /*
mazdo25 3:01b5e80d842d 124 Timeout timeToStop,
mazdo25 3:01b5e80d842d 125 void turn(float degrees) //+ive -> right Turn -ive -> leftTurn
mazdo25 3:01b5e80d842d 126 {
mazdo25 3:01b5e80d842d 127 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 128 if (ratioOfTurn == 0.0f) {return;}
mazdo25 3:01b5e80d842d 129 rightWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 130 leftWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 131 wait(0.1);
mazdo25 3:01b5e80d842d 132 if (degrees > 0 )
mazdo25 3:01b5e80d842d 133 {
mazdo25 3:01b5e80d842d 134 rightWheel->adjustAngularVelocity(rightWheel->maxAngularVel);
mazdo25 3:01b5e80d842d 135 leftWheel->adjustAngularVelocity((leftWheel->maxAngularVel)*-1.0f);
mazdo25 3:01b5e80d842d 136 }
mazdo25 3:01b5e80d842d 137 else
mazdo25 3:01b5e80d842d 138 {
mazdo25 3:01b5e80d842d 139 rightWheel->adjustAngularVelocity((rightWheel->maxAngularVel)*-1.0f);
mazdo25 3:01b5e80d842d 140 leftWheel->adjustAngularVelocity(leftWheel->maxAngularVel);
mazdo25 3:01b5e80d842d 141 }
mazdo25 3:01b5e80d842d 142
mazdo25 3:01b5e80d842d 143 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 144 }
mazdo25 3:01b5e80d842d 145
mazdo25 3:01b5e80d842d 146 void travelDistance(float d)//in metres
mazdo25 3:01b5e80d842d 147 {
mazdo25 3:01b5e80d842d 148 timeToStop.attach(callback(this, &Robot::stopMovement), d/(calculateTranslationalVelocity()*(float)PI*Wheel::wheelDiameter));
mazdo25 3:01b5e80d842d 149 }
mazdo25 3:01b5e80d842d 150
mazdo25 3:01b5e80d842d 151 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 152 {
mazdo25 3:01b5e80d842d 153 sensorArray[sensorNumber]->sample();
mazdo25 3:01b5e80d842d 154
mazdo25 3:01b5e80d842d 155 if (sensorNumber < (numberOfSensors/2))
mazdo25 3:01b5e80d842d 156 {
mazdo25 3:01b5e80d842d 157 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-3);
mazdo25 3:01b5e80d842d 158 }
mazdo25 3:01b5e80d842d 159 else
mazdo25 3:01b5e80d842d 160 }
mazdo25 3:01b5e80d842d 161 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-2);
mazdo25 3:01b5e80d842d 162 }
mazdo25 3:01b5e80d842d 163
mazdo25 3:01b5e80d842d 164 sensorNumber++;
mazdo25 3:01b5e80d842d 165 if (sensorNumber % numberOfSensors == 0)
mazdo25 3:01b5e80d842d 166 {
mazdo25 3:01b5e80d842d 167 sensorNumber = 0;
mazdo25 3:01b5e80d842d 168 controller.setProcessValue(lineVoltages[(lvIndex%numberOfSamples)];
mazdo25 3:01b5e80d842d 169 adjustAngularVelocity(controller.compute());
mazdo25 3:01b5e80d842d 170 lvIndex++;
mazdo25 3:01b5e80d842d 171 }
mazdo25 3:01b5e80d842d 172 }
mazdo25 3:01b5e80d842d 173
mazdo25 3:01b5e80d842d 174 */
mazdo25 4:208f5279143a 175 //lineVoltages[lvIndex] += 0.5f;
mazdo25 4:208f5279143a 176
mazdo25 4:208f5279143a 177
mazdo25 4:208f5279143a 178
mazdo25 4:208f5279143a 179 /*if input value is greater than the maximum value the left wheel can go, go full right TURN
mazdo25 4:208f5279143a 180 if (W > leftWheel->returnMaxAngularVel()*AF)
mazdo25 4:208f5279143a 181 {
mazdo25 4:208f5279143a 182 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 183 rightWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 184 }
mazdo25 4:208f5279143a 185 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 186 {
mazdo25 4:208f5279143a 187 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
mazdo25 4:208f5279143a 188 leftWheel->adjustAngularVelocity(0);
mazdo25 4:208f5279143a 189 }
mazdo25 4:208f5279143a 190 else if (W == 0)
mazdo25 4:208f5279143a 191 {
mazdo25 4:208f5279143a 192 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 193 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
mazdo25 4:208f5279143a 194 }
mazdo25 4:208f5279143a 195 else
mazdo25 4:208f5279143a 196 {
mazdo25 4:208f5279143a 197
mazdo25 4:208f5279143a 198 */