Skip to content

Commit b4b0085

Browse files
unknownunknown
unknown
authored and
unknown
committed
Initial GIT Import
0 parents  commit b4b0085

28 files changed

+3849
-0
lines changed

‎TwichDrone/python/ardumotor.py

+151
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,151 @@
1+
import sys
2+
3+
from nanpy import (ArduinoApi, SerialManager)
4+
from time import sleep
5+
6+
class MotorPINS:
7+
def __init__(self, label, channel, direction, power, brake, current, default):
8+
self.label = label
9+
self.channel = channel
10+
self.direction = direction
11+
self.power = power
12+
self.brake = brake
13+
self.current = current
14+
self.default = default
15+
16+
def setup(self, arduino):
17+
arduino.pinMode(self.direction,arduino.OUTPUT);
18+
arduino.pinMode(self.brake, arduino.OUTPUT);
19+
20+
class ArduMotor:
21+
22+
FORWARD = 1
23+
BACKWARD = 0
24+
25+
# map this acordly
26+
27+
MOTOR_LEFT = "LEFT"
28+
MOTOR_RIGHT = "RIGHT"
29+
30+
DEFAULT_LEFT = BACKWARD # if motor is reversed, fixed it here
31+
DEFAULT_RIGHT = BACKWARD
32+
33+
def __init__(self, port=None, verbose=0):
34+
35+
self.verbose = verbose
36+
37+
try:
38+
if port:
39+
self.connection = SerialManager(device=port)
40+
else:
41+
self.connection = SerialManager()
42+
self.arduino = ArduinoApi(connection=self.connection)
43+
44+
except Exception, e:
45+
print "Problem starting arduino interface: %s [skipping arduino interface]" % e
46+
self.arduino = None
47+
48+
self.motor_left = MotorPINS("LEFT", 'B', 13,11,8, 1,ArduMotor.DEFAULT_LEFT) #channel B
49+
self.motor_right = MotorPINS("RIGHT", 'A', 12,3,9, 0,ArduMotor.DEFAULT_RIGHT) #channel A
50+
51+
self.motors = { ArduMotor.MOTOR_LEFT: self.motor_left, ArduMotor.MOTOR_RIGHT: self.motor_right }
52+
53+
def Setup(self):
54+
if not self.arduino:
55+
return
56+
self.motor_left.setup(self.arduino)
57+
self.motor_right.setup(self.arduino)
58+
59+
def GetCurrent(self, motor):
60+
if not self.arduino:
61+
return 0.0
62+
VOLT_PER_AMP = 1.65
63+
m = self.motors[motor]
64+
65+
currentRaw = self.arduino.analogRead(m.current)
66+
currentVolts = currentRaw *(5.0/1024.0)
67+
currentAmps = currentVolts/VOLT_PER_AMP
68+
69+
#print "Sensing %s:%s: %3.2f" % (m.label, m.channel, currentAmps)
70+
return currentAmps
71+
72+
73+
74+
def DriveMotor(self, motor, direction=None, power=None, brake=False):
75+
if not self.arduino:
76+
return
77+
78+
m = self.motors[motor]
79+
80+
if brake:
81+
self.arduino.digitalWrite(m.brake, self.arduino.HIGH);
82+
return
83+
84+
# sanity check
85+
86+
if direction==None or power==None:
87+
return
88+
89+
if power < 0: power = 0
90+
if power > 255: power = 255
91+
92+
ar_dir = self.arduino.HIGH # forward
93+
if direction == ArduMotor.BACKWARD:
94+
ar_dir = self.arduino.LOW # backward
95+
96+
if m.default != ArduMotor.FORWARD:
97+
# if mounted reversal, change direction here.
98+
if ar_dir == self.arduino.HIGH:
99+
ar_dir = self.arduino.LOW
100+
else:
101+
ar_dir = self.arduino.HIGH
102+
103+
#print "Driving motor: %s:%s to %s: [power: %s] (brake: %s) " % (m.label, m.channel, ar_dir, power, brake)
104+
105+
106+
self.arduino.digitalWrite(m.direction, ar_dir);
107+
self.arduino.digitalWrite(m.brake, self.arduino.LOW);
108+
self.arduino.analogWrite(m.power, power);
109+
110+
111+
def TestMotor(self,motor):
112+
if not self.arduino:
113+
return
114+
115+
if motor not in self.motors.keys():
116+
return ("Error, unknown motor %s" % motor)
117+
118+
for i in [0, 10, 50, 100, 200, 255]:
119+
#print "Motor: %s Speed %s (Forward)" % (motor,i)
120+
self.DriveMotor(motor, ArduMotor.FORWARD, i)
121+
sleep(0.5)
122+
123+
self.DriveMotor(motor,brake=True)
124+
125+
126+
for i in [255, 200, 100, 50, 10, 0]:
127+
#print "Motor: %s Speed %s (Backward)" % (motor,i)
128+
self.DriveMotor(motor, ArduMotor.BACKWARD, i)
129+
sleep(0.5)
130+
131+
self.DriveMotor(motor,brake=True)
132+
133+
134+
135+
136+
if __name__ == "__main__":
137+
138+
# mac with CH341x (Clone)
139+
140+
#driver =ArduMotor() # raspy
141+
#driver =ArduMotor(port='/dev/cu.wchusbserial1a1130') # mac
142+
driver =ArduMotor(port='/dev/cu.wchusbserial620') # macbookpro
143+
144+
driver.Setup()
145+
146+
driver.TestMotor(driver.MOTOR_RIGHT)
147+
sleep(1)
148+
driver.TestMotor(driver.MOTOR_LEFT)
149+
150+
151+

‎TwichDrone/python/ardumotor.pyc

3.92 KB
Binary file not shown.

‎TwichDrone/python/controlserver.py

+145
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,145 @@
1+
#!/usr/bin/python
2+
3+
import thread
4+
import threading
5+
import wsock
6+
import model
7+
import argparse
8+
import sys
9+
import time
10+
from ardumotor import ArduMotor
11+
from nanpy import (ArduinoApi, SerialManager)
12+
from goprohero import GoProHero
13+
import logging
14+
15+
class GoProHelper:
16+
def __init__(self,passwd):
17+
self.camera = GoProHero(password=passwd,log_level=logging.CRITICAL) #info shows LOTS of things
18+
19+
def Status(self):
20+
s= self.camera.status()
21+
if s['raw'] == {}:
22+
s = { 'power': 'off', 'batt1': 0}
23+
return s
24+
#for i in s.keys(): print "%s: %s" % (i, s[i])
25+
26+
def RecordStart(self):
27+
self.camera.command('record', 'on')
28+
29+
def RecordStop(self):
30+
self.camera.command('record', 'off')
31+
32+
def LOG(msg):
33+
s = time.strftime("%Y/%m/%d %H:%M:%S", time.localtime())
34+
msg = "[%s]" + msg
35+
return msg % s
36+
37+
if __name__ == "__main__":
38+
39+
parser = argparse.ArgumentParser()
40+
parser.add_argument("-v", "--verbose", help="Show detailed model info", action="count", default=0)
41+
parser.add_argument("-a", "--address", help="Host Address", default='')
42+
parser.add_argument("-p", "--port", help="Listen Port", default=8000)
43+
parser.add_argument("-f", "--frequency", help="FPS to run model", default=60)
44+
parser.add_argument("-g", "--gopropasswd",help="GoPro Password", default='')
45+
parser.add_argument("-s", "--serialport",help="arduino serial port", default='')
46+
args = parser.parse_args()
47+
48+
model.MODEL = model.DroneModel(verbose=args.verbose)
49+
model.MODEL_LOCK = threading.Lock()
50+
51+
wsock.websocketserver_start(args.address,int(args.port))
52+
53+
print "ControlServer started at %s:%s (verbose:%s) Refresh: %s Hz" % (args.address, args.port, args.verbose, args.frequency)
54+
55+
#TODO: pass as argument
56+
#driver =ArduMotor() # raspy
57+
#driver =ArduMotor(port='/dev/cu.wchusbserial1a1130') # mac
58+
#driver =ArduMotor(port='/dev/cu.wchusbserial620') # macbookpro
59+
60+
driver =ArduMotor(port=args.serialport) # macbookpro
61+
62+
gopro = GoProHelper(args.gopropasswd)
63+
gopro_status = gopro.Status()
64+
print "Gopro Status: Powered: %s [battery: %s %%]" % (gopro_status['power'], gopro_status['batt1'])
65+
66+
olddata = None
67+
ml_amp = 0.0
68+
mr_amp = 0.0
69+
70+
gopro_timer = time.time()
71+
72+
while True:
73+
target_freq = 1/float(args.frequency)
74+
sample_start_time = time.time()
75+
76+
if time.time() - gopro_timer > 10:
77+
gopro_timer = time.time()
78+
if gopro_status['power'] == 'off':
79+
gopro = GoProHelper(args.gopropasswd)
80+
gopro_status = gopro.Status()
81+
82+
83+
# begin block
84+
85+
ml_amp = (ml_amp + driver.GetCurrent(ArduMotor.MOTOR_LEFT) )/2.0
86+
mr_amp = (mr_amp + driver.GetCurrent(ArduMotor.MOTOR_RIGHT) )/2.0
87+
88+
stats = { 'gopro': gopro_status, 'amp_l': ml_amp, 'amp_r': mr_amp }
89+
90+
model.MODEL.set_stats(stats)
91+
model.MODEL.update()
92+
data = model.MODEL.getdata()
93+
94+
if olddata and (olddata['data'] != data['data'] or olddata['MR'] != data['MR'] or olddata['ML'] != data['ML'] or olddata['buttons'] != data['buttons']):
95+
96+
# manage buttons
97+
rec_old = olddata['buttons'] and olddata['buttons']['rec']
98+
rec = None
99+
if data['buttons'] and 'rec' in data['buttons']:
100+
rec = data['buttons']['rec']
101+
102+
if data['buttons']:
103+
if rec_old:
104+
if not rec_old and rec:
105+
if args.verbose >1 : print LOG("START_RECORDING")
106+
gopro.RecordStart()
107+
if rec_old and not rec:
108+
if args.verbose >1 : print LOG("STOP_RECORDING")
109+
gopro.RecordStop()
110+
else:
111+
if rec:
112+
if args.verbose >1 : print LOG("START_RECORDING")
113+
gopro.RecordStart()
114+
else:
115+
if args.verbose >1 : print LOG("STOP_RECORDING")
116+
gopro.RecordStop()
117+
118+
# send data to arduino HERE
119+
120+
driver.DriveMotor(ArduMotor.MOTOR_LEFT, data['ML'].direction , data['ML'].power)
121+
driver.DriveMotor(ArduMotor.MOTOR_RIGHT, data['MR'].direction , data['MR'].power)
122+
123+
# until here
124+
125+
if args.verbose >= 1:
126+
MRD = 'F'
127+
MLD = 'F'
128+
if data['MR'].direction == model.MotorModel.BACKWARD: MRD = 'B'
129+
if data['ML'].direction == model.MotorModel.BACKWARD: MLD = 'B'
130+
131+
print LOG("[CONTROL][OUT][MotorLEFT] [Power: %03d] [Direction: %s[%s] | [MotorRIGHT] [Power: %03d] [Direction: %s[%s]" %\
132+
(data['MR'].power, data['MR'].direction, MRD, data['ML'].power, data['ML'].direction, MLD ))
133+
134+
olddata = data
135+
driver.DriveMotor(ArduMotor.MOTOR_LEFT,brake=True)
136+
driver.DriveMotor(ArduMotor.MOTOR_RIGHT,brake=True)
137+
138+
139+
# time control
140+
sample_end_time = time.time()
141+
time_diff = sample_end_time - sample_start_time
142+
if time_diff < target_freq:
143+
time.sleep(target_freq - time_diff)
144+
145+

‎TwichDrone/python/drone.png

9.87 KB
Loading

‎TwichDrone/python/drone.psd

51.5 KB
Binary file not shown.

0 commit comments

Comments
 (0)