-
Notifications
You must be signed in to change notification settings - Fork 70
/
robot_main_RPI.py
113 lines (90 loc) · 4.05 KB
/
robot_main_RPI.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
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Sun Mar 15 20:31:07 2020
@author: miguel-asd
"""
import numpy as np
import time
import csv
from src.kinematic_model import robotKinematics
from src.joystick import Joystick
from src.serial_com import ArduinoSerial
from src import angleToPulse
from src.gaitPlanner import trotGait
from src.CoM_stabilization import stabilize
##This part of code is just to save the raw telemetry data.
fieldnames = ["t","roll","pitch"]
with open('telemetry/data.csv','w') as csv_file:
csv_writer = csv.DictWriter(csv_file,fieldnames = fieldnames)
csv_writer.writeheader()
def update_data():
with open('telemetry/data.csv','a') as csv_file:
csv_writer = csv.DictWriter(csv_file, fieldnames = fieldnames)
info = {"t" : t,
"roll" : realRoll,
"pitch" : realPitch}
csv_writer.writerow(info)
robotKinematics = robotKinematics()
joystick = Joystick('/dev/input/event1') #need to specify the event route
arduino = ArduinoSerial('/dev/ttyACM0') #need to specify the serial port
trot = trotGait()
control = stabilize()
#robot properties
"""initial safe position"""
#angles
targetAngs = np.array([0 , np.pi/4 , -np.pi/2, 0 ,#BR
0 , np.pi/4 , -np.pi/2, 0 ,#BL
0 , np.pi/4 , -np.pi/2, 0 ,#FL
0 , np.pi/4 , -np.pi/2, 0 ])#FR
#FR_0 to FR_4
#FRcoord = np.matrix([0. , -3.6 , -0.15])
#FLcoord = np.matrix([0. , 3.6 , -0.15])
#BRcoord = np.matrix([0. , -3.6 , -0.15])
#BLcoord = np.matrix([0. , 3.6 , -0.15])
"initial foot position"
#foot separation (0.182 -> tetta=0) and distance to floor
Ydist = 0.18
Xdist = 0.25
height = 0.16
#body frame to foot frame vector (0.08/-0.11 , -0.07 , -height)
bodytoFeet0 = np.matrix([[ 0.085 , -0.075 , -height],
[ 0.085 , 0.075 , -height],
[-0.11 , -0.075 , -height],
[-0.11 , 0.075 , -height]])
orn = np.array([0. , 0. , 0.])
pos = np.array([0. , 0. , 0.])
Upid_yorn = [0.]
Upid_y = [0.]
Upid_xorn = [0.]
Upid_x = [0.]
startTime = time.time()
lastTime = startTime
t = []
T = 0.4 #period of time (in seconds) of every step
offset = np.array([0. , 0.5 , 0.5 , 0.]) #defines the offset between each foot step in this order (FR,FL,BR,BL)
# [0. , 0.25 , 0.75 , 0.5] creep gait
interval = 0.030
for k in range(100000000000):
if (time.time()-lastTime >= interval):
loopTime = time.time() - lastTime
lastTime = time.time()
t = time.time() - startTime
commandPose , commandOrn , V , angle , Wrot , T , compliantMode = joystick.read()
arduinoLoopTime , Xacc , Yacc , realRoll , realPitch = arduino.serialRecive()#recive serial message
forceModule , forceAngle , Vcompliant , collision = control.bodyCompliant(Xacc , Yacc , compliantMode)
#calculates the feet coord for gait, defining length of the step and direction (0º -> forward; 180º -> backward)
bodytoFeet = trot.loop(V + Vcompliant , angle + forceAngle , Wrot , T , offset , bodytoFeet0)
#####################################################################################
##### kinematics Model: Input body orientation, deviation and foot position ####
##### and get the angles, neccesary to reach that position, for every joint ####
FR_angles, FL_angles, BR_angles, BL_angles , transformedBodytoFeet = robotKinematics.solve(orn + commandOrn, pos + commandPose , bodytoFeet)
pulsesCommand = angleToPulse.convert(FR_angles, FL_angles, BR_angles, BL_angles)
arduino.serialSend(pulsesCommand)#send serial command to arduino
# Upid_x , Upid_y , errorX , errorY , Upid_xorn , Upid_yorn = control.centerPoint(realPitch , realRoll)
# orn[0] = Upid_xorn
# orn[1] = Upid_yorn
# pos[0] = Upid_x
# pos[1] = Upid_y
print (loopTime , arduinoLoopTime , realRoll , realPitch)
update_data()