-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathInvertedPendulum.ino
183 lines (146 loc) · 4.54 KB
/
InvertedPendulum.ino
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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
/****************************************
* Controlled Inverted Pendulum *
* *
* Josh Vazquez 2013 *
* *
* Based on Arduino Starter Kit *
* Project 10: Zoetrope *
* PID library by Brett Beauregard *
* *
****************************************/
#include <PID_v1.h>
// PHYSICAL PINS *********************************************************************
// these are used to tell the IC which way to turn the motor
const int directionPin1 = 2;
const int directionPin2 = 3;
// where to send 0-255 signal for motor voltage 0-100%
const int pwmPin = 9;
// rotary encoder attachment
const int encoderPin = A0;
// CONSTANTS *************************************************************************
// encoder angle at top of arc
const int desiredAngle = 0;
// VARIABLES *************************************************************************
int motorDirection = 1;
// encoder sends raw values 0-1023
int encoderPosition = 0;
// degrees 0-359
float encoderAngle = 0;
// -179 <= relativeAngle <= 180: pendulum angle difference from top of arc
float relativeAngle = 0;
// difference between real angle and desired angle
float error = 0;
// PID VARIABLES *********************************************************************
double pidSetpoint, pidInput, pidOutput;
double realOutput;
// create the PID controller
//PID myPID(&pidInput, &pidOutput, &pidSetpoint, 2, 5, 1, DIRECT); // standard tuning
PID myPID(&pidInput, &pidOutput, &pidSetpoint, 4, 4, 0, DIRECT); // debug tuning
/*
// frontend
double Setpoint = desiredAngle;
double Input = encoderPin;
double Output = pwmPin;
unsigned long serialTime;
*/
// reverse the motor direction
void changeDirection() {
if (motorDirection == 1) {
digitalWrite(directionPin1, HIGH);
digitalWrite(directionPin2, LOW);
}
else {
digitalWrite(directionPin1, LOW);
digitalWrite(directionPin2, HIGH);
}
}
void setup() {
// specify output pins being used
pinMode(directionPin1, OUTPUT);
pinMode(directionPin2, OUTPUT);
pinMode(pwmPin, OUTPUT);
// start with 0 motor power
digitalWrite(pwmPin, LOW);
// PID variables
pidInput = 0;
pidSetpoint = desiredAngle;
// turn PID on
myPID.SetMode(AUTOMATIC);
/* limits: fixes "0" returned for angles 0-180. Uses -128.0~128.0 so that the
* values can be boosted by +/-127 to give a motor output range of -255~-127,
* 0, and 127~255. This is because the lower half of 0-255 gives too little
* voltage to the motor for it to spin. */
myPID.SetOutputLimits(-128.0, 128.0);
// get ready to log data to computer
Serial.begin(9600);
}
void loop() {
delay(1);
// read encoder and compute angle
encoderPosition = analogRead(encoderPin);
// translate 0-1023 from encoder -> 0-359 degrees
encoderAngle = encoderPosition / 1024.0 * 360.0;
// with 0 degrees being the top and increasing clockwise, the left half of the
// rotation range is considered to be -179~-1 instead of 181~359.
if (encoderAngle > 180) {
relativeAngle = encoderAngle - 360;
}
else {
relativeAngle = encoderAngle;
}
// difference from desiredAngle, doesn't account for desired angles other than 0
error = relativeAngle;
// do the PID magic
pidInput = error;
myPID.Compute();
if (pidOutput < 0) {
motorDirection = 0;
changeDirection();
}
else {
motorDirection = 1;
changeDirection();
}
// minimum motor power is 50%
if (pidOutput == 0) {
realOutput = 0;
}
else if (pidOutput > 0) {
realOutput = pidOutput + 127;
}
else if (pidOutput < 0) {
realOutput = pidOutput - 127;
}
// if the pendulum is too far off of vertical to recover, turn off the PID and motor
if (error > 45 || error < -45) {
Serial.print("-- Angle out of bounds -- ");
myPID.SetMode(MANUAL);
analogWrite(pwmPin, 0);
pidOutput = 0;
realOutput = 0;
}
else {
myPID.SetMode(AUTOMATIC);
// in bounds
Serial.print("-- MOTOR ACTIVE, write ");
Serial.print(realOutput);
Serial.print(" --");
analogWrite(pwmPin, abs(realOutput));
}
/*
// frontend
if (millis()>serialTime) {
SerialReceive();
SerialSend();
serialTime+=500;
}
*/
// debug logging
Serial.print("Relative angle: ");
Serial.print(relativeAngle, 2);
Serial.print(" -- PID output: ");
Serial.println(pidOutput);
//Serial.print(" -- proportional: ");
//Serial.print(-- integral: ## -- derivative: ##")
delay(100);
}