-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathYB_Pcb_Car.py
153 lines (135 loc) · 4.28 KB
/
YB_Pcb_Car.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
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
#!/usr/bin/env python
# coding: utf-8
import smbus
import time
import math
class YB_Pcb_Car(object):
def get_i2c_device(self, address, i2c_bus):
self._addr = address
if i2c_bus is None:
return smbus.SMBus(1)
else:
return smbus.SMBus(i2c_bus)
def __init__(self):
# Create I2C device.
self._device = self.get_i2c_device(0x16, 1)
def write_u8(self, reg, data):
try:
self._device.write_byte_data(self._addr, reg, data)
except:
print ('write_u8 I2C error')
def write_reg(self, reg):
try:
self._device.write_byte(self._addr, reg)
except:
print ('write_u8 I2C error')
def write_array(self, reg, data):
try:
# self._device.write_block_data(self._addr, reg, data)
self._device.write_i2c_block_data(self._addr, reg, data)
except:
print ('write_array I2C error')
def Ctrl_Car(self, l_dir, l_speed, r_dir, r_speed):
try:
reg = 0x01
data = [l_dir, l_speed, r_dir, r_speed]
self.write_array(reg, data)
except:
print ('Ctrl_Car I2C error')
def Control_Car(self, speed1, speed2):
try:
if speed1 < 0:
dir1 = 0
else:
dir1 = 1
if speed2 < 0:
dir2 = 0
else:
dir2 = 1
self.Ctrl_Car(dir1, int(math.fabs(speed1)), dir2, int(math.fabs(speed2)))
except:
print ('Ctrl_Car I2C error')
def Car_Run(self, speed1, speed2):
try:
self.Ctrl_Car(1, speed1, 1, speed2)
except:
print ('Car_Run I2C error')
def Car_Stop(self):
try:
reg = 0x02
self.write_u8(reg, 0x00)
except:
print ('Car_Stop I2C error')
def Car_Back(self, speed1, speed2):
try:
self.Ctrl_Car(0, speed1, 0, speed2)
except:
print ('Car_Back I2C error')
def Car_Left(self, speed1, speed2):
try:
self.Ctrl_Car(0, speed1, 1, speed2)
except:
print ('Car_Spin_Left I2C error')
def Car_Right(self, speed1, speed2):
try:
self.Ctrl_Car(1, speed1, 0, speed2)
except:
print ('Car_Spin_Left I2C error')
def Car_Spin_Left(self, speed1, speed2):
try:
self.Ctrl_Car(0, speed1, 1, speed2)
except:
print ('Car_Spin_Left I2C error')
def Car_Spin_Right(self, speed1, speed2):
try:
self.Ctrl_Car(1, speed1, 0, speed2)
except:
print ('Car_Spin_Right I2C error')
def Ctrl_Servo(self, id, angle):
try:
reg = 0x03
data = [id, angle]
if angle < 0:
angle = 0
elif angle > 180:
angle = 180
self.write_array(reg, data)
except:
print ('Ctrl_Servo I2C error')
def Car_Action(self, act_num):
speed = 50
t = 1
spin_speed = 45
spin_t = 2
if(act_num == 0):
self.Car_Run(speed, speed)
time.sleep(t)
self.Car_Stop()
print("forward")
elif(act_num == 1):
self.Car_Back(speed, speed)
time.sleep(t)
self.Car_Stop()
print("backward")
elif(act_num == 2):
self.Car_Spin_Left(spin_speed,spin_speed)
time.sleep(spin_t)
self.Car_Stop()
self.Car_Run(speed, speed)
time.sleep(t)
self.Car_Stop()
self.Car_Spin_Right(spin_speed,spin_speed)
time.sleep(spin_t)
self.Car_Stop()
print("spin 4/pi")
elif(act_num == 3):
self.Car_Spin_Right(spin_speed,spin_speed)
time.sleep(spin_t)
self.Car_Stop()
self.Car_Run(speed, speed)
time.sleep(t)
self.Car_Stop()
self.Car_Spin_Left(spin_speed,spin_speed)
time.sleep(spin_t)
self.Car_Stop()
print("spin -4/pi")