def every5sec() :
b = degree_gyro.acc()
global pwm_1
pwm_1 += 0.01
#print "pwm_v1 = %s pwm_v2 = %s degree = %s \n" % (pwm_1, pwm_2, b.pitch())
print "\n\n\n\n\n\n\n\n\n---------------------motor up---------------------\n"
threading.Timer(5, every5sec).start()
def run(self):
global period1
global period3
global acc1
lock = threading.Lock()
c = degree_gyro.acc()
timecheck_list = []
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = c.pitch()
start_time = time.time()
timecheck_list.append(start_time)
while (True):
timecheck_list.append(time.time())
loop_time = timecheck_list[1] - timecheck_list[0]
timecheck_list.pop(0)
acc_pitch_degree = c.pitch()
gyro_pitch_degree = c.gyro_pitch(loop_time, gyro_pitch_degree)
get_gyro_degree = c.gyro_pitch(loop_time, acc_gyro_pitch)
lock.acquire()
acc1 = acc_gyro_pitch = np.sign(get_gyro_degree) * (
(0.97 * abs(get_gyro_degree)) + (0.03 * abs(acc_pitch_degree)))
lock.release()
def main():
manual = Manual_control(name='recv_rc')
global period1
global period3
global np_gyro_degree
global np_acc_degree
global np_acc_gyro
global np_left_motor
global np_right_motor
global np_ML_data
global start_time
pwm_1 = 1.1
pwm_2 = 1.1
print "start"
a = Servo.servo()
b = degree_gyro.acc()
timecheck_list = []
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch()
## matplotlib data initialization ##
"""
np_gyro_degree = np.array([[0, gyro_pitch_degree]])
np_acc_degree = np.array([[0, b.pitch()]])
np_acc_gyro = np.array([[0, acc_gyro_pitch]])
np_left_motor = np.array([[0, pwm_1]])
np_right_motor = np.array([[0, pwm_2]])
"""
np_ML_data = np.array(
[[0, acc_gyro_pitch,
b.pitch(), gyro_pitch_degree, pwm_1, pwm_2]])
manual.daemon = True
manual.start()
start_time = time.time()
timecheck_list.append(start_time)
while True:
start_action = raw_input("\nI'm ready\nAre you ready?(Y / N): ")
if start_action.upper() == "Y":
print "\nGame will be started! "
break
else:
print "\nOK! let me do it again ~"
while (True):
timecheck_list.append(time.time())
loop_time = timecheck_list[1] - timecheck_list[0]
timecheck_list.pop(0)
acc_pitch_degree = b.pitch()
gyro_pitch_degree = b.gyro_pitch(loop_time, gyro_pitch_degree)
get_gyro_degree = b.gyro_pitch(loop_time, acc_gyro_pitch)
acc_gyro_pitch = np.sign(get_gyro_degree) * (
(0.97 * abs(get_gyro_degree)) + (0.03 * abs(acc_pitch_degree)))
## servo part ##
servo_pwm1 = pwm_1 + (int(period3) - 982) * 0.00049
servo_pwm2 = pwm_2 + (int(period1) - 982) * 0.00049
a.servo_1(servo_pwm1)
a.servo_2(servo_pwm2)
## for matplotlib ##
data_time = time.time() - start_time
"""
np_gyro_degree = np.append(np_gyro_degree, [[data_time, gyro_pitch_degree]], axis=0)
np_acc_degree = np.append(np_acc_degree, [[data_time, acc_pitch_degree]], axis=0)
np_acc_gyro = np.append(np_acc_gyro, [[data_time, acc_gyro_pitch]], axis=0)
np_left_motor = np.append(np_left_motor, [[data_time, servo_pwm1]], axis=0)
np_right_motor = np.append(np_right_motor, [[data_time, servo_pwm2]], axis=0)
"""
np_ML_data = np.append(np_ML_data, [[
data_time, acc_gyro_pitch, acc_pitch_degree, gyro_pitch_degree,
servo_pwm1, servo_pwm2
]],
axis=0)
print "<time: %.16s> : degree= %.16s \tpwm_1= %.5s pwm2= %.5s" % (
data_time, acc_gyro_pitch, servo_pwm1, servo_pwm2)
#print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s" % (servo_pwm1, servo_pwm2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree)
time.sleep(0.01)
def main() :
#pitch_aver = 180
a = Servo.servo()
b = degree_gyro.acc()
global count
###ak_count = 0
#f = open("data.txt", 'w')
que = []
acc_que = []
timecheck_list = []
#gyro_pitch_degree = b.pitch()
#acc_gyro_pitch = b.pitch()
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch()
start_time = time.time()
np_gyro_degree = np.array([[0, gyro_pitch_degree]])
np_acc_degree = np.array([[0, b.pitch()]])
np_acc_gyro = np.array([[0, acc_gyro_pitch]])
#every5sec()
every1sec()
timecheck_list.append(time.time())
while(True):
"""
if(ak_count == 0):
acc_gyro_pitch = b.pitch()
"""
#a.servo_1(pwm_1)
#a.servo_2(pwm_2)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, b.pitch(), count)
timecheck_list.append(time.time())
#print "loop time : %s" % (timecheck_list[1] - timecheck_list[0])
loop_time = timecheck_list[1] - timecheck_list[0]
timecheck_list.pop(0)
#count += 1
"""
data = "pwm_v1 = %s pwm_v2 = %s degree = %s \n" % (pwm_1, pwm_2, acc_pitch_degree)
f.write(data)
"""
acc_pitch_degree = b.pitch()
acc_que.append(acc_pitch_degree)
if(len(acc_que) == 10):
acc_pitch_degree = sum(acc_que, 0.0)/len(acc_que)
acc_que.pop(0)
gyro_pitch_degree = b.gyro_pitch(loop_time, gyro_pitch_degree)
acc_gyro_pitch = (0.97 * gyro_pitch_degree) + (0.03 * acc_pitch_degree)
#print "%s vs %s : %s" % (acc_pitch_degree, gyro_pitch_degree, acc_gyro_pitch)
#que.append((acc_gyro_pitch))
## for matplotlib ##
np_gyro_degree = np.append(np_gyro_degree, [[time.time() - start_time, gyro_pitch_degree]], axis=0)
np_acc_degree = np.append(np_acc_degree, [[time.time() - start_time, acc_pitch_degree]], axis=0)
np_acc_gyro = np.append(np_acc_gyro, [[time.time() - start_time, acc_gyro_pitch]], axis=0)
np.save('gyro_degree_Data', np_gyro_degree)
np.save('acc_degree_Data', np_acc_degree)
np.save('accGyro_degree_Data', np_acc_gyro)
"""
## <Control Code> Use Queue & Degree : 0 ~ 360 ##
if(len(que) == 10):
pitch_aver = sum(que,0.0)/len(que)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, pitch_aver, count)
count += 1
que.pop(0)
if(pitch_aver <=180 and pitch_aver >5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(pitch_aver, 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(pitch_aver, 2), pwm_2, pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
#print "180down"
elif(pitch_aver >180 and pitch_aver < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360-pitch_aver, 2))
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(360-pitch_aver, 2), pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
"""
"""
## <Control Code> NO Queue & Degree : 0 ~ 360 ##
if(acc_gyro_pitch <=180 and acc_gyro_pitch >5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2), pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
elif(acc_gyro_pitch >180 and acc_gyro_pitch < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360-acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(360-acc_gyro_pitch, 2), acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
"""
"""
## <Basic_Control Code> NO Queue & Degree : -180 ~ 180 ##
if(acc_gyro_pitch >= -70 and acc_gyro_pitch < -5):
a.servo_1(pwm_1 + (0.42 / 4900.0) * pow(abs(acc_gyro_pitch), 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (0.42 / 4900.0) * pow(abs(acc_gyro_pitch), 2), pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
elif(acc_gyro_pitch < -70 and acc_gyro_pitch >= -180):
a.servo_1(pwm_1 + l_plus_pwm)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + l_plus_pwm, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
elif(acc_gyro_pitch <= 70 and acc_gyro_pitch > 5):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (0.37 / 4900.0) * pow(acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (0.37 / 4900.0) * pow(acc_gyro_pitch, 2), acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180up"
elif(acc_gyro_pitch < 180 and acc_gyro_pitch > 90):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + r_plus_pwm)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + r_plus_pwm, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
time_count2 = time.time()
"""
"""
## <Test_Control Code> NO Queue & Degree : -180 ~ 180 ##
if(acc_gyro_pitch >= -180 and acc_gyro_pitch < -5):
a.servo_1(pwm_1 + (0.4 / 32400.0) * pow(abs(acc_gyro_pitch), 2))
a.servo_2(pwm_2 - (0.18 / 32400.0) * pow(abs(acc_gyro_pitch),2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(abs(acc_gyro_pitch), 2), pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
elif(acc_gyro_pitch <= 180 and acc_gyro_pitch > 5):
a.servo_1(pwm_1 - (0.2 / 32400.0) * pow(acc_gyro_pitch, 2))
a.servo_2(pwm_2 + (0.35 / 32400.0) * pow(acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(acc_gyro_pitch, 2), acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
"""
"""
## <Test2_Control Code> NO Queue & Degree : -180 ~ 180 ##
time_count1 = time.time()
if(acc_gyro_pitch >= -180 and acc_gyro_pitch < -5):
a.servo_1(pwm_1 + (0.4 / 32400.0) * pow(abs(acc_gyro_pitch), 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(abs(acc_gyro_pitch), 2), pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
elif(acc_gyro_pitch <= 180 and acc_gyro_pitch > 5):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (0.35 / 32400.0) * pow(acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(acc_gyro_pitch, 2), acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
"""
## <Basic_Control Code> NO Queue & Degree : -180 ~ 180 ##
if(acc_gyro_pitch < 0 ):
a.servo_1(pwm_1 - 0.5 * acc_gyro_pitch/90 )
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 - 0.5 * acc_gyro_pitch/90 , pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
else:
a.servo_1(pwm_1 )
a.servo_2(pwm_2 )
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 , pwm_2 + 0.15, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
count += 1
time.sleep(0.05)
def main():
manual = Manual_control(name='recv_rc')
global period1
global period3
global np_gyro_degree
global np_acc_degree
global np_acc_gyro
global np_left_motor
global np_right_motor
global np_ML_data
global start_time
pwm_1 = 1.22
pwm_2 = 1.1
print "start"
a = Servo.servo()
b = degree_gyro.acc()
timecheck_list = []
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch()
## matplotlib data initialization ##
"""
np_gyro_degree = np.array([[0, gyro_pitch_degree]])
np_acc_degree = np.array([[0, b.pitch()]])
np_acc_gyro = np.array([[0, acc_gyro_pitch]])
np_left_motor = np.array([[0, pwm_1]])
np_right_motor = np.array([[0, pwm_2]])
"""
np_ML_data = np.array(
[[0, acc_gyro_pitch,
b.pitch(), gyro_pitch_degree, pwm_1, pwm_2]])
manual.daemon = True
manual.start()
start_time = time.time()
timecheck_list.append(start_time)
while (True):
timecheck_list.append(time.time())
loop_time = timecheck_list[1] - timecheck_list[0]
timecheck_list.pop(0)
acc_pitch_degree = b.pitch()
gyro_pitch_degree = b.gyro_pitch(loop_time, gyro_pitch_degree)
get_gyro_degree = b.gyro_pitch(loop_time, acc_gyro_pitch)
degree_sign = np.sign(get_gyro_degree)
if ((degree_sign * np.sign(acc_pitch_degree)) == 1):
acc_gyro_pitch = degree_sign * ((0.97 * abs(get_gyro_degree)) +
(0.03 * abs(acc_pitch_degree)))
elif (get_gyro_degree == 0 and acc_pitch_degree == 0):
acc_gyro_pitch = degree_sign * ((0.97 * abs(get_gyro_degree)) +
(0.03 * abs(acc_pitch_degree)))
else:
if (get_gyro_degree < 90 and get_gyro_degree > -90):
acc_gyro_pitch = 0.97 * get_gyro_degree + 0.03 * acc_pitch_degree
else:
acc_gyro_pitch = degree_sign * (
(0.97 * abs(get_gyro_degree)) +
(0.03 * (360 - abs(acc_pitch_degree))))
## servo part ##
servo_pwm1 = pwm_1 + (int(period3) - 982) * 0.00049
servo_pwm2 = pwm_2 + (int(period1) - 982) * 0.00049
a.servo_1(servo_pwm1)
a.servo_2(servo_pwm2)
## for matplotlib ##
data_time = time.time() - start_time
"""
np_gyro_degree = np.append(np_gyro_degree, [[data_time, gyro_pitch_degree]], axis=0)
np_acc_degree = np.append(np_acc_degree, [[data_time, acc_pitch_degree]], axis=0)
np_acc_gyro = np.append(np_acc_gyro, [[data_time, acc_gyro_pitch]], axis=0)
np_left_motor = np.append(np_left_motor, [[data_time, servo_pwm1]], axis=0)
np_right_motor = np.append(np_right_motor, [[data_time, servo_pwm2]], axis=0)
"""
np_ML_data = np.append(np_ML_data, [[
data_time, acc_gyro_pitch, acc_pitch_degree, gyro_pitch_degree,
servo_pwm1, servo_pwm2
]],
axis=0)
print "<time: %.16s> : degree= %s \tG = %s A = %s" % (
data_time, acc_gyro_pitch, get_gyro_degree, acc_pitch_degree)
#print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s" % (servo_pwm1, servo_pwm2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree)
time.sleep(0.01)
def main():
#pitch_aver = 180
a = Servo.servo()
b = degree_gyro.acc()
global count
#f = open("data.txt", 'w')
que = []
timecheck_list = []
#gyro_pitch_degree = b.pitch()
#acc_gyro_pitch = b.pitch()
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch()
gyro_pitch_degree = 0
# every5sec()
every1sec()
timecheck_list.append(time.time())
while (True):
#a.servo_1(pwm_1)
#a.servo_2(pwm_2)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, b.pitch(), count)
timecheck_list.append(time.time())
#print "loop time : %s" % (timecheck_list[1] - timecheck_list[0])
loop_time = timecheck_list[1] - timecheck_list[0]
timecheck_list.pop(0)
#count += 1
"""
data = "pwm_v1 = %s pwm_v2 = %s degree = %s \n" % (pwm_1, pwm_2, acc_pitch_degree)
f.write(data)
"""
acc_pitch_degree = b.pitch()
gyro_pitch_degree = b.gyro_pitch(loop_time, gyro_pitch_degree)
acc_gyro_pitch = (0.97 * gyro_pitch_degree) + (0.03 * acc_pitch_degree)
#print "%s vs %s : %s" % (acc_pitch_degree, gyro_pitch_degree, acc_gyro_pitch)
que.append((acc_gyro_pitch))
"""
if(len(que) == 10):
pitch_aver = sum(que,0.0)/len(que)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, pitch_aver, count)
count += 1
que.pop(0)
if(pitch_aver <=180 and pitch_aver >5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(pitch_aver, 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(pitch_aver, 2), pwm_2, pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
#print "180down"
elif(pitch_aver >180 and pitch_aver < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360-pitch_aver, 2))
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(360-pitch_aver, 2), pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
"""
"""
if(acc_gyro_pitch <=180 and acc_gyro_pitch >5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2))
a.servo_2(pwm_2)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2), pwm_2, acc_gyro_pitch, count)
#print "180down"
elif(acc_gyro_pitch >180 and acc_gyro_pitch < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360-acc_gyro_pitch, 2))
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(360-acc_gyro_pitch, 2), acc_gyro_pitch, count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, count)
"""
"""
## <Test2_Control Code> NO Queue & Degree : -180 ~ 180 ##
if(acc_gyro_pitch >= -180 and acc_gyro_pitch < -5):
a.servo_1(pwm_1 + (0.4 / 32400.0) * pow(abs(acc_gyro_pitch), 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(abs(acc_gyro_pitch), 2), pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
elif(acc_gyro_pitch <= 180 and acc_gyro_pitch > 5):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (0.35 / 32400.0) * pow(acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(acc_gyro_pitch, 2), acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
"""
## <Basic_Control Code> NO Queue & Degree : -180 ~ 180 ##
if (acc_gyro_pitch >= -70 and acc_gyro_pitch < -5):
a.servo_1(pwm_1 + (0.45 / 4900.0) * pow(abs(acc_gyro_pitch), 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1 + (0.45 / 4900.0) * pow(abs(acc_gyro_pitch), 2), pwm_2,
acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
#print "180down"
elif (acc_gyro_pitch < -70 and acc_gyro_pitch >= -180):
a.servo_1(pwm_1 + l_plus_pwm)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1 + l_plus_pwm, pwm_2, acc_gyro_pitch, gyro_pitch_degree,
acc_pitch_degree, count)
elif (acc_gyro_pitch <= 70 and acc_gyro_pitch > 5):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (0.4 / 4900.0) * pow(acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2 + (0.4 / 4900.0) * pow(acc_gyro_pitch, 2),
acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
#print "180up"
elif (acc_gyro_pitch < 180 and acc_gyro_pitch > 90):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + r_plus_pwm)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2 + r_plus_pwm, acc_gyro_pitch, gyro_pitch_degree,
acc_pitch_degree, count)
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree,
acc_pitch_degree, count)
time.sleep(0.05)
def main() :
#pitch_aver = 180
a = Servo.servo()
b = degree_gyro.acc()
global count
global init_pwm_1
global init_pwm_2
global np_gyro_degree
global np_acc_degree
global np_acc_gyro
global np_left_motor
global np_right_motor
###ak_count = 0
#f = open("data.txt", 'w')
que = []
acc_que = []
timecheck_list = []
#gyro_pitch_degree = b.pitch()
#acc_gyro_pitch = b.pitch()
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch()
pwm_1 = init_pwm_1
pwm_2 = init_pwm_2
start_time = time.time()
## matplotlib data initialization ##
np_gyro_degree = np.array([[0, gyro_pitch_degree]])
np_acc_degree = np.array([[0, b.pitch()]])
np_acc_gyro = np.array([[0, acc_gyro_pitch]])
np_left_motor = np.array([[0, init_pwm_1]])
np_right_motor = np.array([[0, init_pwm_2]])
#every5sec()
every1sec()
timecheck_list.append(time.time())
while(True):
"""
if(ak_count == 0):
acc_gyro_pitch = b.pitch()
"""
#a.servo_1(pwm_1)
#a.servo_2(pwm_2)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, b.pitch(), count)
timecheck_list.append(time.time())
#print "loop time : %s" % (timecheck_list[1] - timecheck_list[0])
loop_time = timecheck_list[1] - timecheck_list[0]
timecheck_list.pop(0)
#count += 1
"""
data = "pwm_v1 = %s pwm_v2 = %s degree = %s \n" % (pwm_1, pwm_2, acc_pitch_degree)
f.write(data)
"""
acc_pitch_degree = b.pitch()
acc_que.append(acc_pitch_degree)
if(len(acc_que) == 10):
acc_pitch_degree = sum(acc_que, 0.0)/len(acc_que)
acc_que.pop(0)
"""
gyro_pitch_degree = b.gyro_pitch(loop_time, acc_gyro_pitch)
acc_gyro_pitch = (0.97 * gyro_pitch_degree) + (0.03 * acc_pitch_degree)
"""
gyro_pitch_degree = b.gyro_pitch(loop_time, gyro_pitch_degree)
acc_gyro_pitch = (0.97 * b.gyro_pitch(loop_time, acc_gyro_pitch)) + (0.03 * acc_pitch_degree)
#print "%s vs %s : %s" % (acc_pitch_degree, gyro_pitch_degree, acc_gyro_pitch)
time_count = time.time()
#que.append((acc_gyro_pitch))
## for matplotlib ##
np_gyro_degree = np.append(np_gyro_degree, [[time.time() - start_time, gyro_pitch_degree]], axis=0)
np_acc_degree = np.append(np_acc_degree, [[time.time() - start_time, acc_pitch_degree]], axis=0)
np_acc_gyro = np.append(np_acc_gyro, [[time.time() - start_time, acc_gyro_pitch]], axis=0)
np_left_motor = np.append(np_left_motor, [[time.time() - start_time, pwm_1]], axis=0)
np_right_motor = np.append(np_right_motor, [[time.time() - start_time, pwm_2]], axis=0)
proc.stdin.write("hello" + "\n")
print "helloe"
cppMessage = proc.stdout.readline().rstrip("\n")
print "cppreturn message ->" + cppMessage + " written by python \n"
## <Basic_Control Code> NO Queue & Degree : -180 ~ 180 ##
if(acc_gyro_pitch >= -70 and acc_gyro_pitch < -5):
pwm_1 = init_pwm_1 + (0.42 / 4900.0) * pow(abs(acc_gyro_pitch), 2)
pwm_2 = init_pwm_2
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
elif(acc_gyro_pitch < -70 and acc_gyro_pitch >= -180):
pwm_1 = init_pwm_1 + l_plus_pwm
pwm_2 = init_pwm_2
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
elif(acc_gyro_pitch <= 70 and acc_gyro_pitch > 5):
pwm_1 = init_pwm_1
pwm_2 = init_pwm_2 + (0.37 / 4900.0) * pow(acc_gyro_pitch, 2)
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
elif(acc_gyro_pitch < 180 and acc_gyro_pitch > 70):
pwm_1 = init_pwm_1
pwm_2 = init_pwm_2 + r_plus_pwm
a.servo_1(pwm_1)
a.servo_2(pwm_2 + r_plus_pwm)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
else:
pwm_1 = init_pwm_1
pwm_2 = init_pwm_2
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
time_count2 = time.time()
count += 1
def main():
#pitch_aver = 180
a = Servo.servo()
b = degree_gyro.acc()
global count
ak_check = 0
#f = open("data.txt", 'w')
que = []
timecheck_list = []
#gyro_pitch_degree = b.pitch()
#acc_gyro_pitch = b.pitch()
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch()
# every5sec()
every1sec()
timecheck_list.append(time.time())
while (True):
#a.servo_1(pwm_1)
#a.servo_2(pwm_2)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, b.pitch(), count)
timecheck_list.append(time.time())
#print "loop time : %s" % (timecheck_list[1] - timecheck_list[0])
loop_time = timecheck_list[1] - timecheck_list[0]
timecheck_list.pop(0)
#count += 1
"""
data = "pwm_v1 = %s pwm_v2 = %s degree = %s \n" % (pwm_1, pwm_2, acc_pitch_degree)
f.write(data)
"""
acc_pitch_degree = b.pitch()
acc_gyro_pitch = b.new_gyro_pitch(loop_time)
#print "%s vs %s : %s" % (acc_pitch_degree, gyro_pitch_degree, acc_gyro_pitch)
que.append((acc_gyro_pitch))
if (len(que) == 10):
pitch_aver = sum(que, 0.0) / len(que)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, pitch_aver, count)
count += 1
que.pop(0)
if (pitch_aver <= 180 and pitch_aver > 5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(pitch_aver, 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1 + (1.0 / 81000.0) * pow(pitch_aver, 2), pwm_2,
pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
#print "180down"
elif (pitch_aver > 180 and pitch_aver < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360 - pitch_aver, 2))
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2 + (7.0 / 648000.0) * pow(360 - pitch_aver, 2),
pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2, pitch_aver, gyro_pitch_degree, acc_pitch_degree,
count)
"""
if(acc_gyro_pitch <=180 and acc_gyro_pitch >5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2), pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
elif(acc_gyro_pitch >180 and acc_gyro_pitch < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360-acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(360-acc_gyro_pitch, 2), acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
"""
time.sleep(0.05)
def main() :
a = Servo.servo()
b = degree_gyro.acc()
global count
global init_pwm_1
global init_pwm_2
global np_gyro_degree
global np_acc_degree
global np_acc_gyro
global np_left_motor
global np_right_motor
global np_ML_data
global start_time
max_episodes = 2000
## store the previous observations in replay memory
replay_buffer = deque()
que = []
acc_que = []
timecheck_list = []
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch()
pwm_1 = init_pwm_1
pwm_2 = init_pwm_2
## matplotlib data initialization ##
np_ML_data = np.array([[0, acc_gyro_pitch, b.pitch(), gyro_pitch_degree, init_pwm_1, init_pwm_2]])
#every5sec()
every1sec()
with tf.Session() as sess:
mainDQN = dqn.DQN(sess, input_size, output_size, name="main")
targetDQN = dqn.DQN(sess, input_size, output_size, name="target")
tf.global_variables_initializer().run()
## initial copy q_net -> target_net
copy_ops = get_copy_var_ops(dest_scope_name="target", src_scope_name="main")
sess.run(copy_cps)
start_time = time.time()
timecheck_list.append(start_time)
for episode in range(max_episodes):
e = 1. / ((episode / 10) + 1)
done = False
step_count = 0
timecheck_list.append(time.time())
timecheck_list.pop(0)
acc_pitch_degree = b.pitch()
gyro_pitch_degree = b.gyro_pitch(loop_time, gyro_pitch_degree)
get_gyro_degree = b.gyro_pitch(loop_time, acc_gyro_pitch)
acc_gyro_pitch = np.sign(get_gyro_degree) * ((0.97 * abs(get_gyro_degree)) + (0.03 * abs(acc_pitch_degree)))
state =
while not done:
if np.random.rand(1) < e:
pwm_1 = "-------------------------------------------------------"
pwm_2 = "-------------------------------------------------------"
else:
pwm_1 = np.argmax(mainDQN.predict(state))
pwm_2 = np.argmax(mainDQN.predict(state))
## Get new state and reward from environment
next_state, reward, done, _ = "-------------------------------------------------------"
if done: # Penalty
reward = -100
else:
if state[0] > -1 and state[0] < 1:
reward = +10
## Save the experience to our buffer
replay_buffer.append((state, action, reward, next_state, done))
if len(replay_buffer) > REPLAY_MEMORY:
replay_buffer.popleft()
state = next_state
step_count += 1
if step_count > 10000:
break
print "Episode: {} steps: {}".format(episode, step_count)
if step_count > 10000:
pass
if episode % 10 == 1: # train every 10 episode
# Get a random batch of experiences.
for _ in range(50):
minibatch = random.sample(replay_buffer, 10)
loss, _ = replay_train(mainDQN, targetDQN, minibatch)
print("Loss: ", loss)
# copy q_net -> target_net
sess.run(copy_ops)
while(True):
timecheck_list.append(time.time())
timecheck_list.pop(0)
acc_pitch_degree = b.pitch()
gyro_pitch_degree = b.gyro_pitch(loop_time, gyro_pitch_degree)
get_gyro_degree = b.gyro_pitch(loop_time, acc_gyro_pitch)
acc_gyro_pitch = np.sign(get_gyro_degree) * ((0.97 * abs(get_gyro_degree)) + (0.03 * abs(acc_pitch_degree)))
time_count = time.time()
## for matplotlib ##
data_time = time.time() - start_time
np_ML_data = np.append(np_ML_data, [[data_time, acc_gyro_pitch, acc_pitch_degree, gyro_pitch_degree, pwm_1, pwm_2]], axis=0)
count += 1
time.sleep(0.01)
if __name__ == '__main__':
try :
main()
except :
print("finish")
np.save('M_L_Data', np_ML_data)
print "time: %s, number of numpy data: %s" % (time.time() - start_time, len(np_ML_data))
def main():
#pitch_aver = 180
a = Servo.servo()
b = degree_gyro.acc()
global count
global init_pwm_1
global init_pwm_2
global np_gyro_degree
global np_acc_degree
global np_acc_gyro
global np_left_motor
global np_right_motor
###ak_count = 0
#f = open("data.txt", 'w')
que = []
acc_que = []
timecheck_list = []
#gyro_pitch_degree = b.pitch()
#acc_gyro_pitch = b.pitch()
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch()
pwm_1 = init_pwm_1
pwm_2 = init_pwm_2
start_time = time.time()
## matplotlib data initialization ##
np_gyro_degree = np.array([[0, gyro_pitch_degree]])
np_acc_degree = np.array([[0, b.pitch()]])
np_acc_gyro = np.array([[0, acc_gyro_pitch]])
np_left_motor = np.array([[0, init_pwm_1]])
np_right_motor = np.array([[0, init_pwm_2]])
#every5sec()
every1sec()
timecheck_list.append(time.time())
while (True):
"""
if(ak_count == 0):
acc_gyro_pitch = b.pitch()
"""
#a.servo_1(pwm_1)
#a.servo_2(pwm_2)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, b.pitch(), count)
timecheck_list.append(time.time())
#print "loop time : %s" % (timecheck_list[1] - timecheck_list[0])
loop_time = timecheck_list[1] - timecheck_list[0]
timecheck_list.pop(0)
#count += 1
"""
data = "pwm_v1 = %s pwm_v2 = %s degree = %s \n" % (pwm_1, pwm_2, acc_pitch_degree)
f.write(data)
"""
acc_pitch_degree = b.pitch()
"""
## Accelerometer value Averaging with a Queue ##
acc_que.append(acc_pitch_degree)
if(len(acc_que) == 10):
acc_pitch_degree = sum(acc_que, 0.0)/len(acc_que)
acc_que.pop(0)
"""
"""
gyro_pitch_degree = b.gyro_pitch(loop_time, acc_gyro_pitch)
acc_gyro_pitch = (0.97 * gyro_pitch_degree) + (0.03 * acc_pitch_degree)
"""
gyro_pitch_degree = b.gyro_pitch(loop_time, gyro_pitch_degree)
get_gyro_degree = b.gyro_pitch(loop_time, acc_gyro_pitch)
acc_gyro_pitch = np.sign(get_gyro_degree) * (
(0.97 * abs(get_gyro_degree)) + (0.03 * abs(acc_pitch_degree)))
#print "%s vs %s : %s" % (acc_pitch_degree, gyro_pitch_degree, acc_gyro_pitch)
time_count = time.time()
#que.append((acc_gyro_pitch))
## for matplotlib ##
np_gyro_degree = np.append(
np_gyro_degree, [[time.time() - start_time, gyro_pitch_degree]],
axis=0)
np_acc_degree = np.append(
np_acc_degree, [[time.time() - start_time, acc_pitch_degree]],
axis=0)
np_acc_gyro = np.append(np_acc_gyro,
[[time.time() - start_time, acc_gyro_pitch]],
axis=0)
np_left_motor = np.append(np_left_motor,
[[time.time() - start_time, pwm_1]],
axis=0)
np_right_motor = np.append(np_right_motor,
[[time.time() - start_time, pwm_2]],
axis=0)
"""
## <Control Code> Use Queue & Degree : 0 ~ 360 ##
if(len(que) == 10):
pitch_aver = sum(que,0.0)/len(que)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, pitch_aver, count)
count += 1
que.pop(0)
if(pitch_aver <=180 and pitch_aver >5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(pitch_aver, 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(pitch_aver, 2), pwm_2, pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
#print "180down"
elif(pitch_aver >180 and pitch_aver < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360-pitch_aver, 2))
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(360-pitch_aver, 2), pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, pitch_aver, gyro_pitch_degree, acc_pitch_degree, count)
"""
"""
## <Control Code> NO Queue & Degree : 0 ~ 360 ##
if(acc_gyro_pitch <=180 and acc_gyro_pitch >5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2), pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
elif(acc_gyro_pitch >180 and acc_gyro_pitch < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360-acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(360-acc_gyro_pitch, 2), acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
`"""
## <Basic_Control Code> NO Queue & Degree : -180 ~ 180 ##
if (acc_gyro_pitch >= -70 and acc_gyro_pitch < -5):
pwm_1 = init_pwm_1 + (l_plus_pwm / 4900.0) * pow(
abs(acc_gyro_pitch), 2)
pwm_2 = init_pwm_2
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree,
acc_pitch_degree, count)
elif (acc_gyro_pitch < -70 and acc_gyro_pitch >= -180):
pwm_1 = init_pwm_1 + l_plus_pwm
pwm_2 = init_pwm_2
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree,
acc_pitch_degree, count)
elif (acc_gyro_pitch <= 70 and acc_gyro_pitch > 5):
pwm_1 = init_pwm_1
pwm_2 = init_pwm_2 + (r_plus_pwm / 4900.0) * pow(acc_gyro_pitch, 2)
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree,
acc_pitch_degree, count)
elif (acc_gyro_pitch < 180 and acc_gyro_pitch > 70):
pwm_1 = init_pwm_1
pwm_2 = init_pwm_2 + r_plus_pwm
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree,
acc_pitch_degree, count)
else:
pwm_1 = init_pwm_1
pwm_2 = init_pwm_2
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (
pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree,
acc_pitch_degree, count)
time_count2 = time.time()
"""
## <Test_Control Code> NcO Queue & Degree : -180 ~ 180 ##
if(acc_gyro_pitch >= -180 and acc_gyro_pitch < -5):
a.servo_1(pwm_1 + (0.4 / 32400.0) * pow(abs(acc_gyro_pitch), 2))
a.servo_2(pwm_2 - (0.18 / 32400.0) * pow(abs(acc_gyro_pitch),2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(abs(acc_gyro_pitch), 2), pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
elif(acc_gyro_pitch <= 180 and acc_gyro_pitch > 5):
a.servo_1(pwm_1 - (0.2 / 32400.0) * pow(acc_gyro_pitch, 2))
a.servo_2(pwm_2 + (0.35 / 32400.0) * pow(acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(acc_gyro_pitch, 2), acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
"""
"""
## <Test2_Control Code> NO Queue & Degree : -180 ~ 180 ##
time_count1 = time.time()
if(acc_gyro_pitch >= -180 and acc_gyro_pitch < -5):
a.servo_1(pwm_1 + (0.4 / 32400.0) * pow(abs(acc_gyro_pitch), 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 + (1.0 / 81000.0) * pow(abs(acc_gyro_pitch), 2), pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
elif(acc_gyro_pitch <= 180 and acc_gyro_pitch > 5):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (0.35 / 32400.0) * pow(acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2 + (7.0 / 648000.0) * pow(acc_gyro_pitch, 2), acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1, pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree, count)
"""
"""
## <Basic_Control Code> NO Queue & Degree : -180 ~ 180 ##
if(acc_gyro_pitch < 0 ):
a.servo_1(pwm_1 - 0.5 * acc_gyro_pitch/90 )
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 - 0.5 * acc_gyro_pitch/90 , pwm_2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
#print "180down"
else:
a.servo_1(pwm_1 )
a.servo_2(pwm_2 )
print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s ---- count : %s" % (pwm_1 , pwm_2 + 0.15, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree ,count)
"""
count += 1
time.sleep(0.01)
manual = Manual_control(name='recv_rc') global period1 global period3 global np_gyro_degree global np_acc_degree global np_acc_gyro global np_left_motor global np_right_motor pwm_1 = 1.22 pwm_2 = 1.1 print "start" bperiod1 =0 bperiod3 =0 a = Servo.servo() b = degree_gyro.acc() timecheck_list = [] pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch() start_time = time.time() ## matplotlib data initialization ## np_gyro_degree = np.array([[0, gyro_pitch_degree]]) np_acc_degree = np.array([[0, b.pitch()]]) np_acc_gyro = np.array([[0, acc_gyro_pitch]]) np_left_motor = np.array([[0, pwm_1]]) np_right_motor = np.array([[0, pwm_2]]) manual.start()
def main():
manual = Manual_control(name='recv_rc')
global period1
global period3
global np_gyro_degree
global np_acc_degree
global np_acc_gyro
global np_left_motor
global np_right_motor
global np_ML_data
global start_time
global acc1
pwm_1 = 1.22
pwm_2 = 1.1
print "start"
a = Servo.servo()
b = degree_gyro.acc()
timecheck_list = []
pitch_aver = acc_gyro_pitch = gyro_pitch_degree = b.pitch()
## matplotlib data initialization ##
np_ML_data = np.array(
[[0, acc_gyro_pitch,
b.pitch(), gyro_pitch_degree, pwm_1, pwm_2]])
manual.daemon = True
manual.start()
start_time = time.time()
timecheck_list.append(start_time)
while (True):
timecheck_list.append(time.time())
loop_time = timecheck_list[1] - timecheck_list[0]
timecheck_list.pop(0)
acc_pitch_degree = b.pitch()
gyro_pitch_degree = b.gyro_pitch(loop_time, gyro_pitch_degree)
get_gyro_degree = b.gyro_pitch(loop_time, acc_gyro_pitch)
acc_gyro_pitch = np.sign(get_gyro_degree) * (
(0.97 * abs(get_gyro_degree)) + (0.03 * abs(acc_pitch_degree)))
## servo part ##
servo_pwm1 = pwm_1 + (int(period3) - 982) * 0.00049
servo_pwm2 = pwm_2 + (int(period1) - 982) * 0.00049
a.servo_1(servo_pwm1)
a.servo_2(servo_pwm2)
## for matplotlib ##
data_time = time.time() - start_time
np_ML_data = np.append(np_ML_data, [[
data_time, acc_gyro_pitch, acc_pitch_degree, gyro_pitch_degree,
servo_pwm1, servo_pwm2
]],
axis=0)
print acc1
#print "<time: %.16s> : degree= %.16s \tpwm_1= %.5s pwm2= %.5s" % (data_time, acc_gyro_pitch, servo_pwm1, servo_pwm2)
#print "pwm_v1 = %s pwm_v2 = %s degree = C: %s\t<-\tG: %s vs A: %s" % (servo_pwm1, servo_pwm2, acc_gyro_pitch, gyro_pitch_degree, acc_pitch_degree)
time.sleep(0.01)
