Added step response python scripts and LCM type

lcmtypes/mbot_wheel_ctrl_t.lcm

+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

measure_motor_params/measure_motors.c

@@ -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

python/plot_step.py

+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()

python/step_test.py

+#! /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