-
Notifications
You must be signed in to change notification settings - Fork 1
/
bot_utilitySensor.py~
127 lines (88 loc) · 3.86 KB
/
bot_utilitySensor.py~
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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
# -*- coding: utf-8 -*-
"""
Created on Thu Jan 28 16:34:01 2016
@author:
"""
import brickpi
import time
def initInterface(interface, motors, touchPort):
interface.initialize()
interface.motorEnable(motors[0])
interface.motorEnable(motors[1])
touch_port = touchPort[0]
touch_port2 = touchPort[1]
interface.sensorEnable(touch_port, brickpi.SensorType.SENSOR_TOUCH)
interface.sensorEnable(touch_port2, brickpi.SensorType.SENSOR_TOUCH)
motorParams = interface.MotorAngleControllerParameters()
motorParams.maxRotationAcceleration = 8.0
motorParams.maxRotationSpeed = 16.0
motorParams.feedForwardGain = 255/20.0
motorParams.minPWM = 18.0
motorParams.pidParameters.minOutput = -255.0
motorParams.pidParameters.maxOutput = 255.0
motorParams.pidParameters.k_p = 240.0
motorParams.pidParameters.k_i = 1309.0909
motorParams.pidParameters.k_d = 11.0
motorParams2 = interface.MotorAngleControllerParameters()
motorParams2.maxRotationAcceleration = 8.0
motorParams2.maxRotationSpeed = 16.0
motorParams2.feedForwardGain = 255/20.0
motorParams2.minPWM = 18.0
motorParams2.pidParameters.minOutput = -255.0
motorParams2.pidParameters.maxOutput = 255.0
motorParams2.pidParameters.k_p = 240.0
motorParams2.pidParameters.k_i = 1252.1739
motorParams2.pidParameters.k_d = 11.5
interface.setMotorAngleControllerParameters(motors[0],motorParams)
interface.setMotorAngleControllerParameters(motors[1],motorParams2)
def goStraight(distCm, interface, motors):
radianPerCm = 0.3625
angle = distCm * radianPerCm
interface.increaseMotorAngleReferences(motors, [angle, angle])
while not interface.motorAngleReferencesReached(motors) :
motorAngles = interface.getMotorAngles(motors)
time.sleep(0.1)
## Maybe add a break after x seconds
## NOT YET TESTED
def goStraightBump(distCm, interface, motors, touchPort):
radianPerCm = 0.3625
backward_dist=5
angle = distCm * radianPerCm
interface.increaseMotorAngleReferences(motors, [angle, angle])
while not interface.motorAngleReferencesReached(motors) :
motorAngles = interface.getMotorAngles(motors)
resultBump = interface.getSensorValue(touchPort[0])
resultBump2 = interface.getSensorValue(touchPort[1])
#print resultBump
#print resultBump2
if (resultBump[0]==1 and resultBump2[0]==1):
# Only the front bumber no need to take care of the sign
goStraight(-backward_dist, interface, motors)
Right90deg(interface, motors)
# goStraight(+5, interface, motors)
elif (resultBump[0]==1):
# Only the front bumber no need to take care of the sign
goStraight(-backward_dist, interface, motors)
Left90deg(interface, motors)
# goStraight(+20, interface, motors)
elif (resultBump2[0]==1):
goStraight(-backward_dist, interface, motors)
Right90deg(interface, motors)
# goStraight(+20, interface, motors)
time.sleep(0.001)
print motorAngles
print interface.motorAngleReferencesReached(motors)
## Maybe add a break after x seconds
## NOT YET TESTED
def Right90deg(interface, motors):
turn(-90, interface, motors)
## NOT YET TESTED
def Left90deg(interface, motors):
turn(90, interface, motors)
def turn(angleDeg, interface, motors):
radianPerDegre = 0.05622222222
angle = angleDeg * radianPerDegre
interface.increaseMotorAngleReferences(motors, [angle, -angle])
while not interface.motorAngleReferencesReached(motors) :
motorAngles = interface.getMotorAngles(motors)
time.sleep(0.001)