Upload New File

Alphabot2-Lab/Line_Follow.py

+#!/usr/bin/python
+# -*- coding:utf-8 -*-
+import RPi.GPIO as GPIO
+from AlphaBot2 import AlphaBot2
+from rpi_ws281x import Adafruit_NeoPixel, Color
+from TRSensors import TRSensor
+import time
+
+Button = 7
+
+GPIO.setmode(GPIO.BCM)
+GPIO.setwarnings(False)
+GPIO.setup(Button,GPIO.IN,GPIO.PUD_UP)
+
+# LED strip configuration:
+LED_COUNT	   = 4		# Number of LED pixels.
+LED_PIN		   = 18		 # GPIO pin connected to the pixels (must support PWM!).
+LED_FREQ_HZ    = 800000  # LED signal frequency in hertz (usually 800khz)
+LED_DMA		   = 5		 # DMA channel to use for generating signal (try 5)
+LED_BRIGHTNESS = 255	 # Set to 0 for darkest and 255 for brightest
+LED_INVERT	   = False	 # True to invert the signal (when using NPN transistor level shift)	
+
+maximum = 100;
+j = 0
+integral = 0;
+last_proportional = 0
+
+def Wheel(pos):
+#	"""Generate rainbow colors across 0-255 positions."""
+	if pos < 85:
+		return Color(pos * 3, 255 - pos * 3, 0)
+	elif pos < 170:
+		pos -= 85
+		return Color(255 - pos * 3, 0, pos * 3)
+	else:
+		pos -= 170
+		return Color(0, pos * 3, 255 - pos * 3)
+
+# Create NeoPixel object with appropriate configuration.
+strip = Adafruit_NeoPixel(LED_COUNT, LED_PIN, LED_FREQ_HZ, LED_DMA, LED_INVERT, LED_BRIGHTNESS)
+# Intialize the library (must be called once before other functions).
+strip.begin()
+strip.setPixelColor(0, Color(100, 0, 0))	   #Red
+strip.setPixelColor(1, Color(0, 100, 0))	   #Blue
+strip.setPixelColor(2, Color(0, 0, 100))	   #Green
+strip.setPixelColor(3, Color(100, 100, 0))	   #Yellow
+strip.show()
+
+TR = TRSensor()
+Ab = AlphaBot2()
+Ab.stop()
+print("Line follow Example")
+time.sleep(0.5)
+for i in range(0,100):
+	if(i<25 or i>= 75):
+		Ab.right()
+		Ab.setPWMA(30)
+		Ab.setPWMB(30)
+	else:
+		Ab.left()
+		Ab.setPWMA(30)
+		Ab.setPWMB(30)
+	TR.calibrate()
+Ab.stop()
+print(TR.calibratedMin)
+print(TR.calibratedMax)
+while (GPIO.input(Button) != 0):
+	position,Sensors = TR.readLine()
+	print(position,Sensors)
+	time.sleep(0.05)
+Ab.forward()
+print("about to follow")
+while True:
+	position,Sensors = TR.readLine()
+	#print(position)
+	if(Sensors[0] >900 and Sensors[1] >900 and Sensors[2] >900 and Sensors[3] >900 and Sensors[4] >900):
+		Ab.setPWMA(0)
+		Ab.setPWMB(0)
+		print("greater than 900")
+	else:
+		print("less than 900")
+		# The "proportional" term should be 0 when we are on the line.
+		proportional = position - 2000
+		print("crash1")
+		# Compute the derivative (change) and integral (sum) of the position.
+		derivative = proportional - last_proportional
+		integral += proportional
+		print("crash2")
+		# Remember the last position.
+		last_proportional = proportional
+		print("crash3")
+		'''
+		// Compute the difference between the two motor power settings,
+		// m1 - m2.  If this is a positive number the robot will turn
+		// to the right.  If it is a negative number, the robot will
+		// turn to the left, and the magnitude of the number determines
+		// the sharpness of the turn.  You can adjust the constants by which
+		// the proportional, integral, and derivative terms are multiplied to
+		// improve performance.
+		'''
+		power_difference = proportional/30	+ integral/10000 + derivative*2;  
+		print("crash4")
+		if (power_difference > maximum):
+			power_difference = maximum
+		if (power_difference < - maximum):
+			power_difference = - maximum
+		print(position,power_difference)
+		if (power_difference < 0):
+			Ab.setPWMA(maximum + power_difference)
+			Ab.setPWMB(maximum);
+		else:
+			Ab.setPWMA(maximum);
+			Ab.setPWMB(maximum - power_difference)
+		print("crash5")
+#	for i in range(0,strip.numPixels()):
+#		strip.setPixelColor(i, Wheel(((i * 256 / strip.numPixels()) + j) & 255))
+#	strip.show();
+	print("crash6")
+	j += 1
+	print("crash7")
+	if(j > 256*4): 
+		j= 0
+		print("crash8")