StableBot/MegaPi_StableBot.cpp at main · aloyak/StableBot · 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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
/*
------------ StableBot ------------ (CODE NEEDS VALUE TUNING!)

MegaPi (MeAuriga V1.3) robot that uses a PID controller to balance itself on two wheels.
With love @AlmartDev :)

*/

#include <MeAuriga.h>

#include <Wire.h>
#include <PID_v1.h>

double setpoint = 0;
double input, output;

double Kp = 23;
double Ki = .8;
double Kd = .2;

PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

MeEncoderOnBoard Encoder_1(SLOT1);
MeEncoderOnBoard Encoder_2(SLOT2);

Gyro gyro(0, 0x69);

void isr_process_encoder1(void)
{
	if (digitalRead(Encoder_1.getPortB()) == 0)
	{
		Encoder_1.pulsePosMinus();
	}
	else
	{
		Encoder_1.pulsePosPlus();
	}
}

void isr_process_encoder2(void)
{
	if (digitalRead(Encoder_2.getPortB()) == 0)
	{
		Encoder_2.pulsePosMinus();
	}
	else
	{
		Encoder_2.pulsePosPlus();
	}
}

void setup()
{
	Serial.begin(9600);
	gyro.begin();

	attachInterrupt(Encoder_1.getIntNum(), isr_process_encoder1, RISING);
	attachInterrupt(Encoder_2.getIntNum(), isr_process_encoder2, RISING);
	Serial.begin(115200);

	// Initialize PID controller (might have to change the values)
    pid.SetMode(AUTOMATIC);
    pid.SetOutputLimits(-255, 255);
    pid.SetSampleTime(10);

	// Set PWM 8KHz
	TCCR1A = _BV(WGM10);
	TCCR1B = _BV(CS11) | _BV(WGM12);

	TCCR2A = _BV(WGM21) | _BV(WGM20);
	TCCR2B = _BV(CS21);
}

void setSpeed(int speed)
{
	Encoder_1.setMotorPwm(-speed);
	Encoder_2.setMotorPwm(speed);

	Encoder_1.updateSpeed();
	Encoder_2.updateSpeed();
}

void loop()
{
	gyro.update();
  	Serial.read();

	input = gyro.getAngleX(); // Using the X-axis to balance the robot

	pid.Compute();

	setSpeed(output);

	// Nice to use with the serial plotter
	Serial.println(input);
	Serial.println(output);

	delay(10);
}