From d851617a9df12ee6971de8d2c99e43a4d4fd6d27 Mon Sep 17 00:00:00 2001 From: Raspberry Pi Date: Sat, 16 Nov 2019 23:03:46 -0600 Subject: Updated encoder to have an offset when zeroing (allows that initial zero can be set to upright instead of hanging). Updated system to allow for different angular units to be used (passed by argument on constructor, then passed to encoder.read_position when used). Bug fixes to system_swingup_test. Swingup test now runs properly (and almost actually did a swing up at one point), couldn't keep testing because the system accidentally destroyed itself, so called it time to stop to let glue dry.... --- System_Python/encoder.py | 44 +++++++++++++++++++----------------- System_Python/system.py | 35 ++++++++++++++++------------ System_Python/system_swingup_test.py | 8 +++---- 3 files changed, 47 insertions(+), 40 deletions(-) diff --git a/System_Python/encoder.py b/System_Python/encoder.py index 8b7053c..63bc73e 100644 --- a/System_Python/encoder.py +++ b/System_Python/encoder.py @@ -14,59 +14,61 @@ class Encoder: # Set the board IO (just in case it hasn't been done yet) GPIO.setmode(GPIO.BCM) # Setup class varaiable - self.offset=0 + self.offset = 0 self.clk_pin = clk_pin self.cs_pin = cs_pin self.data_pin = data_pin # Setup the IO try: - GPIO.setup(self.clk_pin,GPIO.OUT) - GPIO.setup(self.cs_pin,GPIO.OUT) - GPIO.setup(self.data_pin,GPIO.IN) + GPIO.setup(self.clk_pin, GPIO.OUT) + GPIO.setup(self.cs_pin, GPIO.OUT) + GPIO.setup(self.data_pin, GPIO.IN) # Setup the CS and CLK to be high - GPIO.output(PIN_CLK,1) - GPIO.output(PIN_CS,1) + GPIO.output(PIN_CLK, 1) + GPIO.output(PIN_CS, 1) except: - print("ERROR. Unable to setup the configuration required") + # If this fails, it's likely because the IO has already been configured. The encoders share some pins. Ignore the failure + time.sleep(0.01) # Used just to have something in the exception catch + #print("ERROR. Unable to setup the configuration required") # Wait some time to before reading time.sleep(0.5) - def set_zero(self): + def set_zero(self, offset = 0): # Take current position as zero - self.offset=self.read_position('Raw') + self.offset = self.read_position('Raw') - offset def clockup(self): - GPIO.output(self.clk_pin,1) + GPIO.output(self.clk_pin, 1) def clockdown(self): - GPIO.output(self.clk_pin,0) + GPIO.output(self.clk_pin, 0) def read_position(self, format): # Most of this is based of timing diagram of encoder # Pull CS low to start reading - GPIO.output(self.cs_pin,0) + GPIO.output(self.cs_pin, 0) # Delay necessary before reading is ready time.sleep(delay*2) data = 0 # Clockdown necessary before reading self.clockdown() # Go through 10 bits needed to read - for i in range(0,10): + for i in range(0, 10): # Clock up to start reading one bit self.clockup() # Shift data left and insert input - data<<=1 - data|=GPIO.input(self.data_pin) + data <<= 1 + data |= GPIO.input(self.data_pin) # Clock down after finish reading self.clockdown() # Pull CS high after finish reading - GPIO.output(self.cs_pin,1) + GPIO.output(self.cs_pin, 1) # Format with offset, Max is 1024 - data=(data-self.offset)%1024 + data = (data - self.offset) % 1024 # Data is linearly mapped if format=="Raw": return data - elif format=="Degrees": - degrees=(data/1024.0)*360.0 + elif format == "Degrees": + degrees = (data/1024.0) * 360.0 return degrees - elif format=="Radian": - radians=(data/1024.0)*(2.0*math.pi) + elif format == "Radians": + radians = (data/1024.0) * (2.0*math.pi) return radians else: print("ERROR. Invalid format (Raw, Degrees, Radians)") diff --git a/System_Python/system.py b/System_Python/system.py index 8f67872..b5a68cc 100644 --- a/System_Python/system.py +++ b/System_Python/system.py @@ -30,7 +30,7 @@ system_min_x = -16.5 # System Class # This is the primary interface a student will use to control the pendulum. class System: - def __init__(self, negative_limit=float('nan'), positive_limit=float('nan')): + def __init__(self, negative_limit=float('nan'), positive_limit=float('nan'), angular_units='Degrees'): GPIO.setwarnings(False) # Initialize public variables self.max_x = system_max_x @@ -39,7 +39,7 @@ class System: 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() + self.encoder_angular.set_zero(offset = 512) # set offset so that 0 is upright vertical # Initialize the linear encoder. self.encoder_linear = Linear_Encoder(encoder_clock_pin, encoder_linear_cs_pin, encoder_data_pin) # We assume that the system has been initialized on startup to a 0 position, or that the previous run ended by returning the system to 0 @@ -63,12 +63,14 @@ class System: # NOTE: If only one limit has been defined, this should always work (hardware limits will be the absolute edge on the undefined side, although this would be difficult for users to utilize unless we provide the position of the hardware limits on each side # NOTE: If neither limit is defined, the hardware limits will be the only limits in effect. + self.angular_units = angular_units + # Create and setup results file (to be sent back to the server and displayed/downloaded to the user) # Results file is a CSV with the following entries: angle, position, speed self.result_filename = "/home/pi/test_results/" + datetime.now().strftime("%d-%m-%Y_%H:%M:%S") + ".csv" result_file = open(self.result_filename, "w+") - result_file.write("angle(degrees),position(inches),speed(percentage)\n") + result_file.write("angle(" + angular_units + "),position(inches),speed(percentage)\n") result_file.close() # END __init__() @@ -124,9 +126,10 @@ class System: ##### 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 + angular_position = self.encoder_angular.read_position(self.angular_units) + if self.angular_units == 'Degrees': + if angular_position > 180.: + angular_position = angular_position - 360. linear_position = self.encoder_linear.read_position() # Check soft limits if not math.isnan(self.negative_soft_limit) and linear_position < self.negative_soft_limit: @@ -152,10 +155,10 @@ class System: ##### 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 + if speed > 100.: + speed = 100. + if speed < -100.: + speed = -100. # 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() @@ -167,9 +170,9 @@ class System: # open the results file result_file = open(self.result_filename, "a") # Write the results - result_file.write("%d," % angle) # Write angle - result_file.write("%d," % position) # Write position - result_file.write("%d\n" % speed) # Write speed (end of line) + result_file.write("%f," % angle) # Write angle + result_file.write("%f," % position) # Write position + result_file.write("%f\n" % speed) # Write speed (end of line) # Close the results file result_file.close() # END add_results @@ -180,10 +183,12 @@ class System: # slowly move towards 0 until we get there if position > 0: self.motor.move(-4) + while position > 0: + position = self.encoder_linear.read_position() elif position < 0: self.motor.move(4) - while not position == 0: - position = self.encoder_linear.read_position() + while position < 0: + position = self.encoder_linear.read_position() self.motor.brake() # END return_home diff --git a/System_Python/system_swingup_test.py b/System_Python/system_swingup_test.py index e4d3a72..7a9b5bd 100644 --- a/System_Python/system_swingup_test.py +++ b/System_Python/system_swingup_test.py @@ -52,7 +52,7 @@ class SwingUpEnv(): } def __init__(self): - self.sys = System() + self.sys = System(angular_units='Radians') self.force_mag = 10.0 self.tau = 0.02 # seconds between state updates @@ -95,10 +95,10 @@ class SwingUpEnv(): self.up_time = 0 new_theta, new_x = self.sys.measure() - new_theta = radians(new_theta) theta_dot = (new_theta - theta) / self.tau x_dot = (new_x - x) / self.tau self.state = (new_x, x_dot, new_theta, theta_dot) + self.sys.add_results(new_theta, new_x, force) done = x < -self.x_threshold \ or x > self.x_threshold \ @@ -159,7 +159,7 @@ class nnQ(pt.nn.Module): def forward(self,x,a): x = pt.tensor(x, dtype = pt.float32) - b = pt.nn.functional.one_hot(pt.tensor(a), self.numActions) + b = pt.nn.functional.one_hot(pt.tensor(a).long(), self.numActions) c = b.float().detach() y = pt.cat([x, c]) @@ -179,6 +179,7 @@ class sarsaAgent: def action(self, x): # This is an epsilon greedy selection + a = 0 if rnd.rand() < self.epsilon: a = rnd.randint(numActions) else: @@ -252,7 +253,6 @@ while step < maxSteps: y = x_to_y(x) a = agent.action(y) u = Actions[a:a+1] - env.render() x_next, c, done, info = env.step(u) max_up_time = info['max_up_time'] -- cgit v1.2.3