Skip to content
Snippets Groups Projects
Commit 031c52a6 authored by Demo User's avatar Demo User
Browse files

merge conflicts solved

parents 3e7379b0 0ace834f
Branches master
No related tags found
No related merge requests found
struct mbot_wheel_ctrl_t {
struct mbot_wheel_ctrl_t
{
int64_t utime;
float left_motor_pwm_cmd;
......
......@@ -80,7 +80,7 @@ int main(int argc, char** argv){
}
#endif
if(rc_encoder_init()<0){
if(rc_encoder_eqep_init()<0){
fprintf(stderr,"ERROR: failed to initialze encoders\n");
return -1;
}
......@@ -96,6 +96,7 @@ int main(int argc, char** argv){
if(rc_get_state()==RUNNING){
rc_nanosleep(1E9); //sleep for 1s
//TODO: write routine here
}
// TODO: Plase exit routine here
......
import sys
import numpy as np
import matplotlib.pyplot as plt
sys.path.append("lcmtypes")
import lcm
from lcmtypes import odometry_t
if len(sys.argv) < 2:
sys.stderr.write("usage: decode_log.py <logfile>")
sys.exit(1)
log = lcm.EventLog(sys.argv[1],"r")
data = np.empty((0,4), dtype=float)
init = 0
for event in log:
if event.channel == "ODOMETRY":
msg = odometry_t.decode(event.data)
if init==0:
start_utime = msg.utime
init = 1
data = np.append(data, np.array([[ \
(msg.utime-start_utime)/1.0E6, \
msg.x, \
msg.y, \
msg.theta
]]), axis=0)
plt.plot(data[:,1], data[:,2], 'r')
plt.show()
\ No newline at end of file
import sys
import os
import numpy as np
import matplotlib.pyplot as plt
sys.path.append("lcmtypes")
import lcm
from lcmtypes import mbot_wheel_ctrl_t
if len(sys.argv) < 2:
sys.stderr.write("usage: plot_step.py <logfile>")
sys.exit(1)
file = sys.argv[1]
log = lcm.EventLog(file,"r")
data = np.empty((0,7), dtype=float)
init = 0
for event in log:
if event.channel == "MBOT_WHEEL_CTRL":
msg = mbot_wheel_ctrl_t.decode(event.data)
if init==0:
start_utime = msg.utime
init = 1
data = np.append(data, np.array([[ \
(msg.utime-start_utime)/1.0E6, \
msg.left_motor_pwm_cmd, \
msg.right_motor_pwm_cmd, \
msg.left_motor_vel_cmd, \
msg.right_motor_vel_cmd, \
msg.left_motor_vel, \
msg.right_motor_vel
]]), axis=0)
cmd = data[:,3]
start = np.where(cmd>=0.1)[0][0]-5
end = start + 500 #plot 10s worth of data
p1 = plt.plot(data[start:end,0], data[start:end,3], 'g', label="vel cmd")
p2 = plt.plot(data[start:end,0], data[start:end,1], 'r', label="pwm cmd")
p3 = plt.plot(data[start:end,0], data[start:end,5], 'b', label="velocity")
plt.legend(loc="upper right")
filename, file_extension = os.path.splitext(file)
plt.savefig(filename + '.png')
plt.show()
#! /usr/bin/python
import lcm
from time import sleep
import sys
sys.path.append("lcmtypes")
from lcmtypes import mbot_motor_command_t
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=1")
stop_command = mbot_motor_command_t()
stop_command.trans_v = 0.0
stop_command.angular_v = 0.0
drive_command = mbot_motor_command_t()
drive_command.trans_v = 0.25
drive_command.angular_v = 0.0
lc.publish("MBOT_MOTOR_COMMAND",drive_command.encode())
sleep(4.0)
lc.publish("MBOT_MOTOR_COMMAND",stop_command.encode())
sleep(1.0)
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment