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
|
#!/usr/bin/env python
from motor import Motor
from encoder import Encoder
import math
# IO pin definitions
### Motor pins
motor_speed_pin = 17
motor_forward_pin = 27
motor_reverse_pin = 22
### Encoder pins (shared by both encoders)
encoder_clock_pin = 2
encoder_data_pin = 3
### Angular encoder pins
encoder_angular_cs_pin = 4
### Linear encoder pins
encoder_linear_cs_pin = 5
# System Class
# This is the primary interface a student will use to control the pendulum.
class System:
def __init__(self):
# Initialize the motor.
self.motor = Motor(motor_speed_pin, motor_forward_pin, motor_reverse_pin)
# Initialize the angular encoder.
self.encoder_angular = Encoder(encoder_clock_pin, encoder_angular_cs_pin, encoder_data_pin)
self.encoder_angular.set_zero()
# Initialize the linear encoder.
self.encoder_linear = Linear_Encoder(encoder_clock_pin, encoder_linear_cs_pin, encoder_data_pin)
self.encoder_linear.set_zero()
# END __init__()
# Get the values of the encoders to determine the angular and linear position of the pendulum.
# Values are returned as a tuple: (angle, linear).
### angle: 0 indicates the pendulum is exactly straight up.
##### 180 or -180 indicate the pendulum is exactly straight down.
##### Positive values indicate the pendulum is leaning to the right.
##### Negative values indicate the pendulum is leaning to the left.
### linear: 0 indicates the pendulum is exactly in the middle of the track.
##### Positive values indicate the pendulum is right-of-center.
##### Negative values indicate the pendulum is left-of-center.
def measure(self):
angular_position = self.encoder_angular.read_position('Degrees')
if angular_position > 180:
angular_position = angular_position - 360
linear_position = self.encoder_linear.read_position()
return (angular_position, linear_position)
# END measure()
# Adjust the pendulum's linear position using the motor.
### speed: Acceptable values range from -100 to 100 (as a percentage), with 100/-100 being the maximum adjustment speed.
##### Negative values will move the pendulum to the left.
##### Positive values will move the pendulum to the right.
def adjust(self, speed):
# cap the speed inputs
if speed > 100.0:
speed = 100.0
if speed < -100.0:
speed = -100.0
# change the motor speed
# TODO: Make sure the motor is oriented so that positive speed the correct direction (same for negative). Change the values otherwise.
self.motor.coast()
self.motor.move(speed)
# END adjust()
# END System
# Linear Encoder class
# This class is to help with using an absolute encoder for linear position sensing as assembled in the physical system.
# The function definitions here are the same as with the regular encoder (pseudo-interface).
class Linear_Encoder:
PROPORTION = 14.5
def __init__(self, clk_pin, cs_pin, data_pin):
self.encoder = Encoder(clk_pin, cs_pin, data_pin)
self.set_zero()
def set_zero(self):
# Set the zero position for the encoder
self.encoder.set_zero()
# Reset the internal position counter
self.rotations = 0
self.last_position = 0
def read_position(self):
# Read the position of the encoder
position = self.encoder.read_position('Raw')
# Compare to last known position
# NOTE: For now, assume that we are moving the smallest possible distance (i.e. 5 -> 1 is -4, not 1020)
if position - self.last_position > 0:
if position < 512 and self.last_position > 512:
# We are moving to the right (positive) and have completed a new rotation
self.rotations = self.rotations + 1
else:
if position > 512 and self.last_position < 512:
# We are moving to the left (negative) and have completed a new rotation
self.rotations = self.rotations - 1
# Save the last position for the next calculation
self.last_position = position
# compute the position based on the system parameters
# linear position = (2pi*r)(n) + (2pi*r)(position/1024) = (2pi*r)(n + position/1024) = (pi*d)(n + position/1024)
return (PROPORTION)*(self.rotations + position/1024)
|
