Inverted Pendulum on a Cart

1/**
2 * == Inverted pendulum stabilisation with state control with DC  motor ==
3 * 
4 * == Hardware specification ==
5 * OMRON E6B2-CWZ6C pinout
6  * - Brown - Vcc
7 * - Black - Phase A
8 * - White - Phase B
9 * - Orange -  Phaze Z
10 * - Blue - GND
11 *
12 * LPD3806-600BM-G5-24C pinout
13 * - Green  - Phase A
14 * - White - Phase B
15 * - Red - Vcc
16 * - Black - GND
17 */
18
19#include  <Arduino.h>
20
21// motor encoder pins
22#define OUTPUT_A  3 // PE5
23#define  OUTPUT_B  2 // PE4
24
25// pendulum encoder pins
26#define REF_OUT_A 18 // PD3
27#define  REF_OUT_B 19 // PD2
28
29// pulses per revolution
30#define PPR  2400
31#define  SHAFT_R 0.00573
32#define PENDULUM_ENCODER_PPR  10000
33
34#define PWM_PIN 10
35#define  DIR_PIN 8
36
37#define POSITION_LIMIT  0.145
38
39#define A 35.98
40#define  B 2.22
41#define C 2.79
42
43#define Kth 147.1
44#define Kw  36.0
45#define  Kx  54.0
46#define Kv  39.5
47
48const float THETA_THRESHOLD = PI / 12;
49const  float PI2 = 2.0 * PI;
50
51volatile long encoderValue = 0L;
52volatile long lastEncoded  = 0L;
53
54volatile long refEncoderValue = 0;
55volatile long lastRefEncoded  = 0;
56
57unsigned long now = 0L;
58unsigned long lastTimeMicros = 0L;
59
60float  x, last_x, v, dt;
61float theta, last_theta, w;
62float control, u;
63
64unsigned  long log_prescaler = 0;
65
66void encoderHandler();
67void refEncoderHandler();
68
69void  setup() {
70
71  // setting PWD frequency on pin 10 to 31kHz
72  TCCR2B = (TCCR2B  & 0b11111000) | 0x01;
73
74  pinMode(OUTPUT_A, INPUT_PULLUP);
75  pinMode(OUTPUT_B,  INPUT_PULLUP);
76
77  pinMode(REF_OUT_A, INPUT_PULLUP);
78  pinMode(REF_OUT_B,  INPUT_PULLUP);
79
80  attachInterrupt(digitalPinToInterrupt(OUTPUT_A), encoderHandler,  CHANGE);
81  attachInterrupt(digitalPinToInterrupt(OUTPUT_B), encoderHandler, CHANGE);
82
83  attachInterrupt(digitalPinToInterrupt(REF_OUT_A), refEncoderHandler, CHANGE);
84  attachInterrupt(digitalPinToInterrupt(REF_OUT_B), refEncoderHandler, CHANGE);
85
86  pinMode(PWM_PIN, OUTPUT);
87  pinMode(DIR_PIN, OUTPUT);
88
89  digitalWrite(DIR_PIN,  LOW);
90
91  Serial.begin(9600);
92  lastTimeMicros = 0L;
93}
94
95float saturate(float  v, float maxValue) {
96  if (fabs(v) > maxValue) {
97    return (v > 0) ? maxValue  : -maxValue;
98  } else {
99    return v;
100  }
101}
102
103float getAngle(long  pulses, long ppr) {  
104  float angle = (PI + PI2 * pulses / ppr);
105  while (angle  > PI) {
106    angle -= PI2;
107  }
108  while (angle < -PI) {
109    angle += PI2;
110  }
111  return angle;
112}
113
114float getCartDistance(long pulses, long ppr)  {
115  return 2.0 * PI * pulses / PPR * SHAFT_R;
116}
117
118void driveMotor(float  u) {
119  digitalWrite(DIR_PIN, u > 0.0 ? LOW : HIGH);
120  analogWrite(PWM_PIN,  fabs(u));
121}
122
123boolean isControllable(float theta) {
124  return fabs(theta)  < THETA_THRESHOLD;
125}
126
127void log_state(float control, float u) {
128  if  (fabs(w) > 100) {
129    return;
130  }
131
132  if (log_prescaler % 20 == 0) {
133    Serial.print(theta, 4);Serial.print("\	");
134    Serial.print(w, 4);Serial.print("\	");
135    Serial.print(x, 4);Serial.print("\	");
136    Serial.print(v, 4);Serial.print("\	");
137    Serial.print(control, 4);Serial.print("\	");
138    Serial.println(u, 4);
139  }
140  log_prescaler++;
141}
142
143void loop() {
144  now = micros();
145  dt  = 1.0 * (now - lastTimeMicros) / 1000000;
146  x = getCartDistance(encoderValue,  PPR);
147  v = (x - last_x) / dt;
148
149  theta = getAngle(refEncoderValue, PENDULUM_ENCODER_PPR);
150  w = (theta - last_theta) / dt;
151
152  if (isControllable(theta) && fabs(x) <  POSITION_LIMIT) {
153    control = (Kx * x + Kv * v + Kth * theta + Kw * w);
154    u = (control + A * v + copysignf(C, v)) / B;
155    u = 255.0 * u / 12.0;
156    driveMotor(saturate(u, 254));
157  } else {
158    driveMotor(0);
159  }
160  
161  last_x = x;
162  last_theta = theta;
163  lastTimeMicros = now;
164    
165  log_state(control, u);
166  
167  delay(5);
168}
169
170/**
171 * Motor encoder  handler
172 */
173void encoderHandler() {
174  int MSB = (PINE & (1 << PE5)) >>  PE5; //MSB = most significant bit
175  int LSB = (PINE & (1 << PE4)) >> PE4; //LSB  = least significant bit
176  int encoded = (MSB << 1) | LSB; //converting the 2  pin value to single number
177  int sum  = (lastEncoded << 2) | encoded; //adding  it to the previous encoded value
178
179  if(sum == 0b1101 || sum == 0b0100 || sum  == 0b0010 || sum == 0b1011) {
180    encoderValue++; //CW
181  }
182  if(sum ==  0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) {
183    encoderValue--;  //CCW
184  }
185
186  lastEncoded = encoded; //store this value for next time  
187}
188
189/**
190  * Pendulum encoder handler
191 * Encoder attached to pins:
192 * Phase A - 18 PD3
193  * Phase B - 19 PD2
194 */
195void refEncoderHandler() {
196  int MSB = (PIND & (1  << PD3)) >> PD3; //MSB = most significant bit
197  int LSB = (PIND & (1 << PD2))  >> PD2; //LSB = least significant bit
198  int encoded = (MSB << 1) | LSB; //converting  the 2 pin value to single number
199  int sum  = (lastRefEncoded << 2) | encoded;  //adding it to the previous encoded value
200
201  if(sum == 0b1101 || sum == 0b0100  || sum == 0b0010 || sum == 0b1011) {
202    refEncoderValue++; //CW
203  }
204  if(sum  == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) {
205    refEncoderValue--;  //CCW
206  }
207
208  lastRefEncoded = encoded; //store this value for next time  
209}