Commit 7ed0e320 authored by Demo User's avatar Demo User
Browse files

merge odometry

parents b7fd65a9 cc8103ed
......@@ -12,6 +12,23 @@
int mb_initialize_controller(){
//mb_load_controller_config();
left_wheel_velocity_pid = rc_filter_empty();
right_wheel_velocity_pid = rc_filter_empty();
left_wheel_low_pass = rc_filter_empty();
right_wheel_low_pass = rc_filter_empty();
//put variable for left pid and right pid
float kp = 0.65;
float ki = 0.02;
float kd = 0.15;
rc_filter_pid(&left_wheel_velocity_pid,kp,ki,kd,0.1,DT);
rc_filter_pid(&right_wheel_velocity_pid,kp,ki,kd,0.1,DT);
rc_filter_enable_saturation(&left_wheel_velocity_pid,-1.0,1.0);
rc_filter_enable_saturation(&right_wheel_velocity_pid,-1.0,1.0);
rc_filter_first_order_lowpass(&left_wheel_low_pass,DT,0.1);
rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.1);
return 0;
}
......@@ -60,7 +77,15 @@ int mb_load_controller_config(){
*
*******************************************************************************/
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
//mb_state->left_velocity = rc_filter_march(&left_wheel_low_pass,mb_state->left_velocity);
//mb_state->right_velocity = rc_filter_march(&right_wheel_low_pass, mb_state->right_velocity);
float left_error = mb_setpoints->fwd_velocity - LEFT_MOTOR_POL*mb_state->left_velocity;
float right_error = mb_setpoints->fwd_velocity - RIGHT_MOTOR_POL*mb_state->right_velocity;
mb_state->left_cmd = rc_filter_march(&left_wheel_velocity_pid, left_error);
mb_state->right_cmd = rc_filter_march(&right_wheel_velocity_pid, right_error);
// mb_state->left_cmd = rc_filter_march(&left_wheel_low_pass,mb_state->left_cmd);
//mb_state->right_cmd = rc_filter_march(&right_wheel_low_pass, mb_state->right_cmd );
return 0;
}
......@@ -75,5 +100,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
*******************************************************************************/
int mb_destroy_controller(){
rc_filter_free(&left_wheel_velocity_pid);
rc_filter_free(&right_wheel_velocity_pid);
return 0;
}
......@@ -16,7 +16,10 @@ int mb_destroy_controller();
* rc_filter_t left_wheel_speed_pid;
* rc_filter_t fwd_vel_sp_lpf;
*************/
rc_filter_t left_wheel_velocity_pid;
rc_filter_t right_wheel_velocity_pid;
rc_filter_t left_wheel_low_pass;
rc_filter_t right_wheel_low_pass;
/***********
* For each PID filter you want to load from settings
* add a pid_parameter_t or filter_parameter_t
......
......@@ -18,12 +18,22 @@
#define RIGHT_MOTOR 2 // id of right motor
// TODO: Add convienient defines to define things like motor and encoder polarity here
#define LEFT_MOTOR_POL 1
#define RIGHT_MOTOR_POL -1
#define LEFT_ENC_POL 1
#define RIGHT_ENC_POL -1
// TODO: Fill in physical propeties of robot
<<<<<<< HEAD
#define GEAR_RATIO 78.0 // gear ratio of motor
#define ENCODER_RES 20.0 // encoder counts per motor shaft revolution
#define WHEEL_DIAMETER 0.084 // diameter of wheel in meters
#define WHEEL_BASE 0.158 // wheel separation distance in meters
=======
#define GEAR_RATIO 78 // gear ratio of motor
#define ENCODER_RES 20 // encoder counts per motor shaft revolution
#define WHEEL_DIAMETER 0.08 // diameter of wheel in meters
#define WHEEL_BASE 0.0 // wheel separation distance in meters
>>>>>>> withlivecode
#define MAX_FWD_VEL 0.8 // maximum forwad speed (m/s)
#define MAX_TURN_VEL 2.5 // maximum turning speed (rad/s)
......@@ -46,6 +56,7 @@
#define MBOT_ENCODER_CHANNEL "MBOT_ENCODERS"
#define MBOT_MOTOR_COMMAND_CHANNEL "MBOT_MOTOR_COMMAND"
#define MBOT_TIMESYNC_CHANNEL "MBOT_TIMESYNC"
#define MBOT_WHEEL_CTRL_CHANNEL "MBOT_CTRL_COMMAND"
#define LCM_ADDRESS "udpm://239.255.76.67:7667?ttl=1"
#define LED_OFF 1
......
......@@ -19,9 +19,13 @@
*
*******************************************************************************/
void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float theta){
<<<<<<< HEAD
mb_odometry->x = x;
mb_odometry->y = y;
mb_odometry->theta = theta;
=======
>>>>>>> origin/withlivecode
}
......@@ -33,7 +37,7 @@ void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float
*
*******************************************************************************/
void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
// mb_odometry is the input of previous veh's position and orientation
// mb_odometry is the input of previenc2metersous veh's position and orientation
// Reference: https://www.hmc.edu/lair/ARW/ARW-Lecture01-Odometry.pdf P28
float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
float del_left = mb_state->left_encoder_delta * 1.0 * enc2meters;
......
......@@ -20,11 +20,11 @@
#include <rc/motor.h>
#include "../common/mb_motor.h"
#include "../common/mb_defs.h"
#include "../common/mb_structs.h"
float enc2meters = (WHEEL_DIAMETER * M_PI) / (GEAR_RATIO * ENCODER_RES);
void test_speed(float du, float dtime_s);
void test_speed(float duty, float dtime_s,mb_state_t* mb);
/*******************************************************************************
* int main()
......@@ -59,11 +59,10 @@ int main(){
}
#endif
if(rc_encoder_eqep_init()<0){
if(rc_encoder_init()<0){
fprintf(stderr,"ERROR: failed to initialze encoders\n");
return -1;
}
// make PID file to indicate your project is running
// due to the check made on the call to rc_kill_existing_process() above
// we can be fairly confident there is no PID file already and we can
......@@ -75,15 +74,43 @@ int main(){
if(rc_get_state()==RUNNING){
rc_nanosleep(1E9); //sleep for 1s
//TODO: write routine here
}
// TODO: Plase exit routine here
rc_encoder_eqep_write(1,0);
rc_encoder_eqep_write(2,0);
mb_state_t mb;
mb.left_encoder_delta = 0;
mb.right_encoder_delta = 0;
//set up file for output
FILE *fpL;
FILE *fpR;
fpL = fopen("left_speed.txt", "w+");
fpR = fopen("right_speed.txt", "w+");
for (float pwm = 0.0; pwm <=1.0; pwm += 0.05){
rc_encoder_eqep_write(1,0);
rc_encoder_eqep_write(2,0);
test_speed(pwm,1E9,&mb);
fprintf(fpL,"%f\t%f\n",pwm,mb.left_velocity);
fprintf(fpR,"%f\t%f\n",pwm,mb.right_velocity);
}
// TODO: Plase exit routine here
fclose(fpL);
fclose(fpR);
rc_encoder_cleanup();
rc_motor_cleanup();
rc_set_state(EXITING);
// remove pid file LAST
rc_remove_pid_file();
return 0;
}
void test_speed(float duty, float dtime_s){
}
\ No newline at end of file
void test_speed(float duty, float dtime_s,mb_state_t* mb){
rc_motor_set(1,duty);
rc_motor_set(2,-duty);
rc_nanosleep(2E8);
rc_nanosleep(dtime_s);
mb->left_encoder_delta= rc_encoder_eqep_read(1);
mb->right_encoder_delta= rc_encoder_eqep_read(2);
mb->left_velocity = mb->left_encoder_delta*1.0 * enc2meters / (dtime_s/1E9);
mb->right_velocity = mb->right_encoder_delta*1.0 * enc2meters / (dtime_s/1E9);
}
......@@ -151,7 +151,9 @@ void read_mb_sensors(){
mb_state.right_encoder_total += mb_state.right_encoder_delta;
rc_encoder_write(LEFT_MOTOR,0);
rc_encoder_write(RIGHT_MOTOR,0);
float enc2meters = (WHEEL_DIAMETER*3.1415) / (GEAR_RATIO*ENCODER_RES);
mb_state.left_velocity = enc2meters * mb_state.left_encoder_delta / DT;
mb_state.right_velocity = enc2meters * mb_state.right_encoder_delta / DT;
//unlock state mutex
pthread_mutex_unlock(&state_mutex);
......@@ -166,7 +168,12 @@ void read_mb_sensors(){
void publish_mb_msgs(){
mbot_imu_t imu_msg;
mbot_encoder_t encoder_msg;
<<<<<<< HEAD
odometry_t odo_msg;
=======
mbot_wheel_ctrl_t wheel_ctrl_msg;
// odometry_t odo_msg;
>>>>>>> origin/withlivecode
//Create IMU LCM Message
imu_msg.utime = now;
......@@ -185,6 +192,15 @@ void publish_mb_msgs(){
encoder_msg.leftticks = mb_state.left_encoder_total;
encoder_msg.rightticks = mb_state.right_encoder_total;
wheel_ctrl_msg.utime = now;
wheel_ctrl_msg.left_motor_pwm_cmd = mb_state.left_cmd;
wheel_ctrl_msg.right_motor_pwm_cmd = mb_state.right_cmd;
wheel_ctrl_msg.left_motor_vel_cmd =mb_setpoints.fwd_velocity;
wheel_ctrl_msg.right_motor_vel_cmd =mb_setpoints.fwd_velocity;
wheel_ctrl_msg.left_motor_vel = -1*mb_state.left_velocity;
wheel_ctrl_msg.right_motor_vel = mb_state.right_velocity;
//TODO: Create Odometry LCM message
odo_msg.utime = now;
odo_msg.x = mb_odometry.x;
......@@ -194,7 +210,12 @@ void publish_mb_msgs(){
//publish IMU & Encoder Data to LCM
mbot_imu_t_publish(lcm, MBOT_IMU_CHANNEL, &imu_msg);
mbot_encoder_t_publish(lcm, MBOT_ENCODER_CHANNEL, &encoder_msg);
<<<<<<< HEAD
odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
=======
mbot_wheel_ctrl_t_publish(lcm,"MBOT_WHEEL_CTRL",&wheel_ctrl_msg);
// odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
>>>>>>> origin/withlivecode
}
/*******************************************************************************
......@@ -213,10 +234,16 @@ void publish_mb_msgs(){
void mobilebot_controller(){
update_now();
read_mb_sensors();
<<<<<<< HEAD
// calculate odometry after update reading and defining sensors readings during each call-back
// mb_odometry x,y,theta will be updated in mb_odoemtry.c
mb_update_odometry(&mb_odometry, &mb_state) ;
=======
mb_controller_update(&mb_state,&mb_setpoints);
>>>>>>> origin/withlivecode
publish_mb_msgs();
rc_motor_set(LEFT_MOTOR,LEFT_MOTOR_POL*mb_state.left_cmd);
rc_motor_set(RIGHT_MOTOR,RIGHT_MOTOR_POL*mb_state.right_cmd);
}
......@@ -257,6 +284,7 @@ void motor_command_handler(const lcm_recv_buf_t *rbuf, const char *channel,
const mbot_motor_command_t *msg, void *user){
mb_setpoints.fwd_velocity = msg->trans_v;
mb_setpoints.turn_velocity = msg->angular_v;
}
......
......@@ -36,6 +36,7 @@
#include "../lcmtypes/oled_message_t.h"
#include "../lcmtypes/timestamp_t.h"
#include "../lcmtypes/reset_odometry_t.h"
#include "../lcmtypes/mbot_wheel_ctrl_t.h"
#include "../common/mb_defs.h"
#include "../common/mb_structs.h"
......
......@@ -33,6 +33,7 @@ for event in log:
cmd = data[:,3]
start = np.where(cmd>=0.1)[0][0]-5
end = start + 500 #plot 10s worth of data
plt.figure(0)
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")
......@@ -40,3 +41,8 @@ plt.legend(loc="upper right")
filename, file_extension = os.path.splitext(file)
plt.savefig(filename + '.png')
plt.show()
plt.figure(1)
p1 = plt.plot(data[start:end,0], data[start:end,4], 'g', label="vel cmd")
p2 = plt.plot(data[start:end,0], data[start:end,2], 'r', label="pwm cmd")
p3 = plt.plot(data[start:end,0], data[start:end,6], 'b', label="velocity")
plt.show()
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