Commit 0ace834f authored by pgaskell's avatar pgaskell
Browse files

Added step response python scripts and LCM type

parent b19f16d1
struct mbot_wheel_ctrl_t
{
int64_t utime;
float left_motor_pwm_cmd;
float right_motor_pwm_cmd;
float left_motor_vel_cmd;
float right_motor_vel_cmd;
float left_motor_vel;
float right_motor_vel;
}
\ No newline at end of file
......@@ -59,7 +59,7 @@ int main(){
}
#endif
if(rc_encoder_init()<0){
if(rc_encoder_eqep_init()<0){
fprintf(stderr,"ERROR: failed to initialze encoders\n");
return -1;
}
......@@ -75,6 +75,7 @@ int main(){
if(rc_get_state()==RUNNING){
rc_nanosleep(1E9); //sleep for 1s
//TODO: write routine here
}
// TODO: Plase exit routine here
......
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
Supports Markdown
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