line-follower/LineFollower.cpp at master · SRJC-Computer-Science-Club/line-follower · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#include "LineFollower.h"
#include "SensorArray.h"
#include "PIDController.h"

namespace LFRobot
{
	LineFollower::LineFollower()
	{
		sensorArray = new SensorArray(PIN_SENSORS, N_SENSORS, MICROS_WHITE, MICROS_BLACK, MICROS_TIMEOUT);
		motors = new MotorPair();
	}

	LineFollower::~LineFollower()
	{
		delete sensorArray;
		delete motors;
	}

	void LineFollower::followLine()
	{


		//was ((1.0f, 0.0f, 10.0f)
		PIDController controller(0.8f, 0.0f, 12.0f);

		controller.start(0);

		while (true)
		{
			float error = sensorArray->getLineOffset();

			/*if (error == 2 || error == -2)
			{
				if (error == 2)
				{
					digitalWrite(PIN_MOTOR_RIGHT_FORWARD, LOW);
					digitalWrite(PIN_MOTOR_RIGHT_BACKWARD, HIGH);


					analogWrite(PIN_MOTOR_RIGHT_PWM, 255);

					digitalWrite(PIN_MOTOR_LEFT_FORWARD, HIGH);
					digitalWrite(PIN_MOTOR_LEFT_BACKWARD, HIGH);


				}
			}
			else {*/

				float correction = controller.getCorrection(error);

				motors->moveForward(ROBOT_SPEED, correction);
			}
		//}
	}

	void LineFollower::testSensorArray()
	{
		while (true)
		{
			Serial.println(sensorArray->getLineOffset());
			delay(100);
		}
	}
}