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@3:01b5e80d842d, 2019-03-09 (annotated)

Committer:
mazdo25
Date:
Sat Mar 09 14:27:48 2019 +0000
Revision:
3:01b5e80d842d
Child:
4:208f5279143a
Initialization working, sensors 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 3:01b5e80d842d 6
mazdo25 3:01b5e80d842d 7 Wheel* leftWheel;
mazdo25 3:01b5e80d842d 8 Wheel* rightWheel;
mazdo25 3:01b5e80d842d 9 lineSensor* sensorArray[numberOfSensors];
mazdo25 3:01b5e80d842d 10 PID controller;
mazdo25 3:01b5e80d842d 11 float lineVoltages[numberOfSamples];
mazdo25 3:01b5e80d842d 12 int lvIndex;
mazdo25 3:01b5e80d842d 13 int sensorNumber;
mazdo25 3:01b5e80d842d 14 Ticker updater;
mazdo25 3:01b5e80d842d 15
mazdo25 3:01b5e80d842d 16 public:
mazdo25 3:01b5e80d842d 17
mazdo25 3:01b5e80d842d 18 Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(1,0,10,0.1)
mazdo25 3:01b5e80d842d 19 {
mazdo25 3:01b5e80d842d 20 lvIndex = 1;
mazdo25 3:01b5e80d842d 21 sensorArray[0] = SA[0];
mazdo25 3:01b5e80d842d 22 leftWheel = LW;
mazdo25 3:01b5e80d842d 23 rightWheel= RW;
mazdo25 3:01b5e80d842d 24
mazdo25 3:01b5e80d842d 25 lineVoltages[0] = -16.5f;
mazdo25 3:01b5e80d842d 26 //controller will output a value between +- max speed of wheels
mazdo25 3:01b5e80d842d 27 controller.setOutputLimits(-1.0f*leftWheel->returnMaxAngularVel(),rightWheel->returnMaxAngularVel());
mazdo25 3:01b5e80d842d 28 controller.setInputLimits(-16.5f, 16.5f); //max limits from sensor readings
mazdo25 3:01b5e80d842d 29 controller.setMode(1);
mazdo25 3:01b5e80d842d 30 controller.setSetPoint(0);
mazdo25 3:01b5e80d842d 31 //based on the fact that it takes 0.2 seconds to take a reading per sensor combo and so we should update our buggy every time it takes a reading from every single sensor
mazdo25 3:01b5e80d842d 32 updater.attach(callback(this, &Robot::robotUpdates),1);
mazdo25 3:01b5e80d842d 33 };
mazdo25 3:01b5e80d842d 34
mazdo25 3:01b5e80d842d 35
mazdo25 3:01b5e80d842d 36 float calculateTranslationalVelocity()
mazdo25 3:01b5e80d842d 37 {
mazdo25 3:01b5e80d842d 38 return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
mazdo25 3:01b5e80d842d 39 }
mazdo25 3:01b5e80d842d 40
mazdo25 3:01b5e80d842d 41 float calculateAngularVelocity()
mazdo25 3:01b5e80d842d 42 {
mazdo25 3:01b5e80d842d 43 return (((leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity())/distanceBetweenWheels));
mazdo25 3:01b5e80d842d 44 }
mazdo25 3:01b5e80d842d 45
mazdo25 3:01b5e80d842d 46 //attempts to modify the angular velocity of the buggy
mazdo25 3:01b5e80d842d 47 void adjustAngularVelocity(float W)
mazdo25 3:01b5e80d842d 48 {
mazdo25 3:01b5e80d842d 49 //if input value is greater than the maximum value the left wheel can go, go full right TURN
mazdo25 3:01b5e80d842d 50 if (W > leftWheel->returnMaxAngularVel())
mazdo25 3:01b5e80d842d 51 {
mazdo25 3:01b5e80d842d 52 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
mazdo25 3:01b5e80d842d 53 rightWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 54 }
mazdo25 3:01b5e80d842d 55 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 3:01b5e80d842d 56 {
mazdo25 3:01b5e80d842d 57 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
mazdo25 3:01b5e80d842d 58 leftWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 59 }
mazdo25 3:01b5e80d842d 60 else if (W == 0)
mazdo25 3:01b5e80d842d 61 {
mazdo25 3:01b5e80d842d 62 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.8f);
mazdo25 3:01b5e80d842d 63 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*0.8f);
mazdo25 3:01b5e80d842d 64 }
mazdo25 3:01b5e80d842d 65 else
mazdo25 3:01b5e80d842d 66 {
mazdo25 3:01b5e80d842d 67 //i actually have no idea what is going on here anymore
mazdo25 3:01b5e80d842d 68 float temp = W - calculateAngularVelocity();
mazdo25 3:01b5e80d842d 69 if (W < 0)
mazdo25 3:01b5e80d842d 70 {
mazdo25 3:01b5e80d842d 71 leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()*0.8f-rightWheel->returnAngularVelocity()) + temp);
mazdo25 3:01b5e80d842d 72 rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.8f);
mazdo25 3:01b5e80d842d 73 }
mazdo25 3:01b5e80d842d 74 else
mazdo25 3:01b5e80d842d 75 {
mazdo25 3:01b5e80d842d 76 rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()*0.8f-leftWheel->returnAngularVelocity()) + temp);
mazdo25 3:01b5e80d842d 77 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*0.8f);
mazdo25 3:01b5e80d842d 78 }
mazdo25 3:01b5e80d842d 79 }
mazdo25 3:01b5e80d842d 80
mazdo25 3:01b5e80d842d 81 }
mazdo25 3:01b5e80d842d 82
mazdo25 3:01b5e80d842d 83 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 84 {
mazdo25 3:01b5e80d842d 85 lineVoltages[lvIndex] += 0.5f;
mazdo25 3:01b5e80d842d 86 controller.setProcessValue(lineVoltages[lvIndex]);
mazdo25 3:01b5e80d842d 87 adjustAngularVelocity(controller.compute());
mazdo25 3:01b5e80d842d 88 }
mazdo25 3:01b5e80d842d 89
mazdo25 3:01b5e80d842d 90 void stopMovement(void)
mazdo25 3:01b5e80d842d 91 {
mazdo25 3:01b5e80d842d 92 leftWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 93 rightWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 94 }
mazdo25 3:01b5e80d842d 95
mazdo25 3:01b5e80d842d 96 };
mazdo25 3:01b5e80d842d 97
mazdo25 3:01b5e80d842d 98
mazdo25 3:01b5e80d842d 99 /*
mazdo25 3:01b5e80d842d 100 Timeout timeToStop,
mazdo25 3:01b5e80d842d 101 void turn(float degrees) //+ive -> right Turn -ive -> leftTurn
mazdo25 3:01b5e80d842d 102 {
mazdo25 3:01b5e80d842d 103 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 104 if (ratioOfTurn == 0.0f) {return;}
mazdo25 3:01b5e80d842d 105 rightWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 106 leftWheel->adjustAngularVelocity(0);
mazdo25 3:01b5e80d842d 107 wait(0.1);
mazdo25 3:01b5e80d842d 108 if (degrees > 0 )
mazdo25 3:01b5e80d842d 109 {
mazdo25 3:01b5e80d842d 110 rightWheel->adjustAngularVelocity(rightWheel->maxAngularVel);
mazdo25 3:01b5e80d842d 111 leftWheel->adjustAngularVelocity((leftWheel->maxAngularVel)*-1.0f);
mazdo25 3:01b5e80d842d 112 }
mazdo25 3:01b5e80d842d 113 else
mazdo25 3:01b5e80d842d 114 {
mazdo25 3:01b5e80d842d 115 rightWheel->adjustAngularVelocity((rightWheel->maxAngularVel)*-1.0f);
mazdo25 3:01b5e80d842d 116 leftWheel->adjustAngularVelocity(leftWheel->maxAngularVel);
mazdo25 3:01b5e80d842d 117 }
mazdo25 3:01b5e80d842d 118
mazdo25 3:01b5e80d842d 119 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 120 }
mazdo25 3:01b5e80d842d 121
mazdo25 3:01b5e80d842d 122 void travelDistance(float d)//in metres
mazdo25 3:01b5e80d842d 123 {
mazdo25 3:01b5e80d842d 124 timeToStop.attach(callback(this, &Robot::stopMovement), d/(calculateTranslationalVelocity()*(float)PI*Wheel::wheelDiameter));
mazdo25 3:01b5e80d842d 125 }
mazdo25 3:01b5e80d842d 126
mazdo25 3:01b5e80d842d 127 void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
mazdo25 3:01b5e80d842d 128 {
mazdo25 3:01b5e80d842d 129 sensorArray[sensorNumber]->sample();
mazdo25 3:01b5e80d842d 130
mazdo25 3:01b5e80d842d 131 if (sensorNumber < (numberOfSensors/2))
mazdo25 3:01b5e80d842d 132 {
mazdo25 3:01b5e80d842d 133 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-3);
mazdo25 3:01b5e80d842d 134 }
mazdo25 3:01b5e80d842d 135 else
mazdo25 3:01b5e80d842d 136 }
mazdo25 3:01b5e80d842d 137 lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-2);
mazdo25 3:01b5e80d842d 138 }
mazdo25 3:01b5e80d842d 139
mazdo25 3:01b5e80d842d 140 sensorNumber++;
mazdo25 3:01b5e80d842d 141 if (sensorNumber % numberOfSensors == 0)
mazdo25 3:01b5e80d842d 142 {
mazdo25 3:01b5e80d842d 143 sensorNumber = 0;
mazdo25 3:01b5e80d842d 144 controller.setProcessValue(lineVoltages[(lvIndex%numberOfSamples)];
mazdo25 3:01b5e80d842d 145 adjustAngularVelocity(controller.compute());
mazdo25 3:01b5e80d842d 146 lvIndex++;
mazdo25 3:01b5e80d842d 147 }
mazdo25 3:01b5e80d842d 148 }
mazdo25 3:01b5e80d842d 149
mazdo25 3:01b5e80d842d 150 */