Commit 031c52a6 authored by Demo User's avatar Demo User
Browse files

merge conflicts solved

parents 3e7379b0 0ace834f
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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment