1+ # Write your code here :-)
12#you have to adjust this part of code for your microproceesorr.
23import time
34from adafruit_crickit import crickit
5+ 46ss = crickit.seesaw
57port1 = crickit.SIGNAL1
68port2 = crickit.SIGNAL2
79port3 = crickit.SIGNAL3
810port4 = crickit.SIGNAL4
911port5 = crickit.SIGNAL5
12+ motor_1 = crickit.dc_motor_1
13+ motor_2 = crickit.dc_motor_2
1014
1115ss.pin_mode(port1,ss.INPUT_PULLUP)
1216ss.pin_mode(port2,ss.INPUT_PULLUP)
1317ss.pin_mode(port3,ss.INPUT_PULLUP)
1418ss.pin_mode(port4,ss.INPUT_PULLUP)
1519ss.pin_mode(port5,ss.INPUT_PULLUP)
16- 1720I=0
1821previousError=0
19- initialPower=0,7
22+ initialPower=1.0
2023#Untill
21- 2224def returnIrSensor(port):
2325 sensor=ss.digital_read(port)
2426 return sensor
25- 2627def getErrorvalue(sensor1,sensor2,sensor3,sensor4,sensor5):
2728 E=0
2829 if sensor1==False and sensor2==False and sensor3==False and sensor4==False and sensor5==True:
@@ -31,36 +32,50 @@ def getErrorvalue(sensor1,sensor2,sensor3,sensor4,sensor5):
3132 E=3
3233 elif sensor1==False and sensor2==False and sensor3==False and sensor4==True and sensor5==False:
3334 E=2
34- elif sensor1==False and sensor2==False and sensor3==True and sensor4==True and sensor5==True :
35+ elif sensor1==False and sensor2==False and sensor3==True and sensor4==True and sensor5==False :
3536 E=1
3637 elif sensor1==False and sensor2==False and sensor3==True and sensor4==False and sensor5==False:
3738 E=0
3839 elif sensor1==False and sensor2==True and sensor3==True and sensor4==False and sensor5==False:
3940 E=-1
4041 elif sensor1==False and sensor2==True and sensor3==False and sensor4==False and sensor5==False:
4142 E=-2
42- elif sensor1==False and sensor2==True and sensor3==True and sensor4==False and sensor5==False:
43+ elif sensor1==True and sensor2==True and sensor3==False and sensor4==False and sensor5==False:
4344 E=-3
44- elif sensor1==True and sensor2==True and sensor3==True and sensor4==False and sensor5==False:
45+ elif sensor1==True and sensor2==False and sensor3==False and sensor4==False and sensor5==False:
4546 E=-4
47+ 4648 return E
4749#This code return a value which is scaled to 1(max)-0(min)
4850#it is adjustig wheel speed with PID value P(error), I(summ of error), D(differential error)
4951def motorPower(P,I,D,initialPower):
50- kp=0,1
51- ki=0,1
52- kd=0,1
52+ kp=1.00
53+ ki=0.000600
54+ kd=2
55+ 5356 PIDvalue = (kp*P) + (ki*I) + (kd*D)
54- leftMotorSpeed=initialPower+PIDvalue
55- rightMotorSpeed=initialPower- PIDvalue
57+ leftMotorSpeed=(- initialPower) +PIDvalue
58+ rightMotorSpeed=( initialPower)+ PIDvalue
5659 return leftMotorSpeed,rightMotorSpeed
57- 5860def motorAction(leftMotorSpeed,rightMotorSpeed):
59- #In this part, you should arrange LeftMotorSpeed and rightMotorspeed 1 maxiumum 0 is stop, you are going to take leftMotorSpeed,rightMotorSpeed
60- #and you are going to scale them between 0 and 1.
61+ #In this part, you should arrange LeftMotorSpeed and rightMotorspeed 1 maxiumum 0 is stop, you are going to take leftMotorSpeed,rightMotorSpeed
62+ #and you are going to scale them between 0 and 1.
63+ #then put
64+ if rightMotorSpeed>=1.00:
65+ rightMotorSpeed=1
66+ if rightMotorSpeed<=0:
67+ rightMotorSpeed=0
68+ if leftMotorSpeed<=-1.00:
69+ leftMotorSpeed=-1.00
70+ if leftMotorSpeed>=0:
71+ leftMotorSpeed=0
72+ 73+ 74+ time.sleep(0.1)
75+ motor_2.throttle=-leftMotorSpeed
76+ motor_1.throttle=-rightMotorSpeed
6177
6278def main(I,previousError):
63- print("start")
6479 #you have to adjust this part of code for your microproceesorr.
6580 sensor1=returnIrSensor(port1)
6681 sensor2=returnIrSensor(port2)
@@ -73,12 +88,9 @@ def main(I,previousError):
7388 I=I+E
7489 D=E-previousError
7590 leftMotorSpeed,rightMotorSpeed = motorPower(P,I,D,initialPower)
76- 77- 91+ motorAction(leftMotorSpeed,rightMotorSpeed)
7892 previousError=E
7993 return I,previousError
8094
81- if __name__ == '__main__':
82- 83- while True:
84- I,previousError=main(I,previousError)
95+ while True:
96+ I,previousError=main(I,previousError)
0 commit comments