Going Straight

GoPiGo
1

I received my GoPiGo a few days ago, assembled it and started playing around, lots of fun!
I was a bit annoyed though: couldn’t get the robot to go straight forward, with or without PID, and regardless of having tried many trim settings.
I wrote the following program that seems to make it go straight, thought I’d post it here and see if others find it useful / have solved it in a different way / improve on it:


#!/usr/bin/env python
#############################################################################################################                                                                  
# Rafi: Trying to move forward while reading the encoders and correcting for bias
##############################################################################################################

from gopigo import *	#Has the basic functions for controlling the GoPiGo Robot
import sys	#Used for closing the running program
import signal

def signalHandler(signum, frame):
	if signum in [signal.SIGINT, signal.SIGTERM]:
		global stop_now
		stop_now = True

stop_now = False
signal.signal(signal.SIGINT, signalHandler)
signal.signal(signal.SIGTERM, signalHandler)
lenc = enc_read(0)
renc = enc_read(1)
speed = 100
left_speed = speed #start with left speed the same as right speed, adapt as needed
trim = 0.0
set_speed(speed)
time.sleep(0.1)
motor_fwd()
while not stop_now:
	time.sleep(0.1)
	nlenc = enc_read(0)
	nrenc = enc_read(1)
	print('left: ' + str(nlenc - lenc) + ' right: ' + str(nrenc - renc))
	if (nlenc != -1) and (lenc != -1) and (nrenc != -1) and (renc != -1): # all reading are valid
		if (nrenc > renc): # right wheel moved at all?
			trim = float(nlenc - lenc) / (nrenc - renc)
		else:
			trim = 100 # sort of infinite
		print ('trim: ' + str(trim))
		# start simple: if left is faster, make it a bit slower. Not sensitive to how much faster
		if (trim < 0.95):
			left_speed += 10
			if (left_speed > speed * 2):
				left_speed = speed * 2
			set_left_speed(left_speed)
			print('increasing left_speed to ' + str(left_speed))
		elif (trim > 1.05):
			left_speed -= 10
			if (left_speed < speed / 2):
				left_speed = speed / 2
			set_left_speed(left_speed)
			print('decreasing left_speed to ' + str(left_speed))
	lenc = nlenc
	renc = nrenc

print('stop')
stop()

Regards,
Rafi

2

Hmmm… Posting code doesn’t work that well.
Attached instead

3

Hey rafit,
The code looks awesome. We had something similar sitting in the firmware and controlling the motors but it did not work well for a lot of folks. It would be great if you could add a before and after video of how your GoPiGo works with and without the code.

We’ll be happy to add this to the Github repo so that other people can use it too.

Thanks,
Karan

4

Sorry for the long delay, I was busy doing real work :slight_smile:
Here’s a video of the robot using fwd() to move:


And here’s a video using the course_correct() function:

B.t.w. I’ve updated the code to be a bit more stand-alone, as follows:


#!/usr/bin/env python
#############################################################################################################                                                                  
# Rafi: Trying to move forward while reading the encoders and correcting for bias
##############################################################################################################

from gopigo import *	#Has the basic functions for controlling the GoPiGo Robot

class Move(object):
	def __init__(self, calibrate = 0.8):
		self.speed = 0
		self.calibrate = calibrate
		self.right_speed = 0
		self.left_speed = 0
		self.heading = 1.0
		stop()
		self.lenc = enc_read(0)
		self.renc = enc_read(1)

	def stop(self):
		self.speed = 0
		stop()

	def move(self, heading = 1.0, speed = 100):
		print('move in direction ' + str(heading))
		self.heading = heading
		self.speed = speed
		if (self.calibrate * self.heading >= 1):
			self.left_speed = self.speed
			self.right_speed = int(self.speed / (self.calibrate * self.heading))
		else:
			self.left_speed = int(self.speed * self.calibrate * self.heading)
			self.right_speed = self.speed
		self.lenc = enc_read(0)
		self.renc = enc_read(1)
		print('setting left_speed to: ' + str(self.left_speed) + ' right_speed to: ' + str(self.right_speed))
		motor_fwd()
		set_left_speed(self.left_speed)
		set_right_speed(self.right_speed)

	def veer_right(self, amount):
		if ( (self.left_speed + amount) <= self.speed):
			self.left_speed += amount
			set_left_speed(self.left_speed)
			print ('veer right by increasing left speed by ' + str(amount) + ' to ' + str(self.left_speed))
		elif (self.right_speed >= amount):
			self.right_speed -= amount
			set_right_speed(self.right_speed)
			print ('veer right by decreasing right speed by ' + str(amount) + ' to ' + str(self.right_speed))
		else:
			print ('could not veer right')

	# def veer_right(self,amount):
	# 	self.right_speed -= amount
	# 	set_right_speed(self.right_speed)
	# 	print ('veer right by decreasing right speed by ' + str(amount) + ' to ' + str(self.right_speed))

	def veer_left(self, amount):
		if ( (self.right_speed + amount) < self.speed):
			self.right_speed += amount
			set_right_speed(self.right_speed)
			print ('veer left by increasing right speed by ' + str(amount) + ' to ' + str(self.right_speed))
		elif (self.left_speed > amount):
			self.left_speed -= amount
			set_left_speed(self.left_speed)
			print ('veer left by decreasing left speed by ' + str(amount) + ' to ' + str(self.left_speed))
		else:
			print ('could not veer left')

	def course_correct(self):
		if self.speed == 0:
			return
		
		nlenc = enc_read(0)
		nrenc = enc_read(1)
		print('left: ' + str(nlenc - self.lenc) + ' right: ' + str(nrenc - self.renc))
		if ( (nlenc - self.lenc) == 0 and (nrenc - self.renc) == 0 ):
			# we should be moving but we're stopped, retry moving forward
			self.move(self.heading, self.speed)
			return
		
		if (nlenc != -1) and (self.lenc != -1) and (nrenc != -1) and (self.renc != -1): # all reading are valid
			rdiff = nrenc - self.renc
			ldiff = nlenc - self.lenc
			if (self.heading == 1 and abs(rdiff - ldiff) <= 1):
				#just continue
				print("going straight and diff is close enough")
			else:
				if (nrenc > self.renc): # right wheel moved at all?
					trim = float(nlenc - self.lenc) / (nrenc - self.renc)
				else:
					trim = 100 # sort of infinite
				print ('trim: ' + str(trim))
				if (trim < self.heading):
					self.veer_right(2)
				elif (trim > self.heading):
					self.veer_left(2)
				self.lenc = nlenc
				self.renc = nrenc
		else:
			self.lenc = nlenc
			self.renc = nrenc
5

Hey Rafi,
Thanks a lot for sharing the new code and the video, it does look like the performance of the GoPiGo is much better compared to the normal code and it is even better since it runs on the software so other people can tinker with it too easily.

We’ll try the new code and would definitely like to merge it with the GoPiGo repository do that other people can use it too.

Thanks again,
Karan