From 1735daf053a553fca613e459357e68d711234de7 Mon Sep 17 00:00:00 2001 From: KenwoodFox <31870999+KenwoodFox@users.noreply.github.com> Date: Thu, 8 Nov 2018 09:43:16 -0500 Subject: [PATCH] Updated libraries and made the linefollower PID into a lib --- .../TumbleBot Line Follower Basic/source.c | 14 +++++++- libraries/lineFollowing.h | 34 ++++++++----------- 2 files changed, 28 insertions(+), 20 deletions(-) diff --git a/Joe/Tumbler Bot/TumbleBot Line Follower Basic/source.c b/Joe/Tumbler Bot/TumbleBot Line Follower Basic/source.c index 2c537c5..b6f9969 100644 --- a/Joe/Tumbler Bot/TumbleBot Line Follower Basic/source.c +++ b/Joe/Tumbler Bot/TumbleBot Line Follower Basic/source.c @@ -18,6 +18,18 @@ int threshold = 2400; int var = 0; +void findLine() +{ + int lowerThreshold = 1200; //thresholdbetween light and dark + + while(SensorValue[masterReflector] < lowerThreshold) + { + standard(27); //go forward at 40 + delay(10); //wait 10ms before checking again + } //this loop will exit once the robot finds the line! + halt(); //stop motors +} + task main() { while(true) @@ -35,7 +47,7 @@ task main() { while(var < 100) { - linePID(21, LED, 0.0078); //run the PID loop with format (SPEED, STATUSLIGHT, P, I, D) + linePID(starboardReflector, masterReflector, portReflector, starboardAft, portAft, LED, 21, 0.0078); //run the PID loop with format (SPEED, STATUSLIGHT, P, I, D) var++; } var = 0; diff --git a/libraries/lineFollowing.h b/libraries/lineFollowing.h index d4718ed..5697e20 100644 --- a/libraries/lineFollowing.h +++ b/libraries/lineFollowing.h @@ -1,21 +1,17 @@ -void findLine() -{ - int lowerThreshold = 1200; //thresholdbetween light and dark +/* This function by Joe. + * If you're reading this, great! Heres a quick guide to get you started, + * use the command linePID(RIGHTLF, CENTERLF, LEFTLF, RIGHTMOTOR, LEFTMOTOR, LED, TGTSPEED, KP); + * and populate the feilds in caps with your variables, if you need to use two motors (tank drive) + * call the function twice, one for each set of left and right wheels, a patch will be coming out + * to fix this. Happy coding! + */ - while(SensorValue[masterReflector] < lowerThreshold) - { - standard(27); //go forward at 40 - delay(10); //wait 10ms before checking again - } //this loop will exit once the robot finds the line! - halt(); //stop motors -} - -void linePID(int speed, char statusLight, float Kp, /*proportional*/ /*float Ki, /*integral*/ /*float Kd, /*derivative*/) //serious maths +void linePID(char rightReflector, char centerReflector, char leftReflector, char rightMotor, char leftMotor, char statusLight, int speed, float Kp, ) //serious maths { int threshold = 400; //The value at witch we can differ a black line from a white table, we could make this a self calibrating value!! - float error = SensorValue[starboardReflector] - SensorValue[portReflector]; //rescan the error + float error = SensorValue[rightReflector] - SensorValue[leftReflector]; //rescan the error /* In a PID loop, the goal is to aproach the target, but not overshoot it, a good PID loop can swoosh right to the target, * while many others simply just, rush past it and come back right after, this mathamtical form of position control has been * around since the first basic electronic navigation systems @@ -24,8 +20,8 @@ void linePID(int speed, char statusLight, float Kp, /*proportional*/ /*float Ki, if((abs(error)) <= 10) //While the absolute value of the error (pos || neg) is less than the deadzone, do nothing, just drive ahead { SensorValue[statusLight] = true; //Turn on the LED while tracking center - portDriveTrain(speed); //drive at speed - starboardDriveTrain(speed); //drive at speed + motor[leftMotor] = speed; //drive at speed + motor[rightMotor] = speed; //drive at speed /* It is well understood that the motors very often do not drive at the same rate, so its likely we'll wobble or hug one side of * line, this is not all that bad tbh */ @@ -33,13 +29,13 @@ void linePID(int speed, char statusLight, float Kp, /*proportional*/ /*float Ki, else //In the case of being in error (Never gonna happen ik), do the following { SensorValue[statusLight] = false; //turn off the light - starboardDriveTrain(speed + (Kp * error)); //Subtract or add to the value speed, the error multplied by its weight (Kp) - portDriveTrain(speed + (Kp * (error * -1))); //Subtract or add to the value speed, the error multplied by its weight (Kp) + motor[rightMotor] = (speed + (Kp * error)); //Subtract or add to the value speed, the error multplied by its weight (Kp) + motor[leftMotor] = (speed + (Kp * (error * -1))); //Subtract or add to the value speed, the error multplied by its weight (Kp) /* we've multiplied the weight(kp) by error, but the error number is negative in the starboard side so that we can stray backwards * and away from the line when we impact it on the corrosponding side */ - if(SensorValue[masterReflector] >= threshold && error <= 200) + if(SensorValue[centerReflector] >= threshold && error <= 200) { SensorValue[LED] = true; //LED on /* Just a test, if the value of master reflector is greater than the threshold AND the error is less than a number @@ -51,4 +47,4 @@ void linePID(int speed, char statusLight, float Kp, /*proportional*/ /*float Ki, */ } } -} \ No newline at end of file +}