Commit 1c4e81b6 authored by gxiang's avatar gxiang
Browse files

added professor code

parent a0185e26
......@@ -75,6 +75,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
float right_error = mb_setpoints->fwd_velocity - 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);
return 0;
}
......
......@@ -18,14 +18,14 @@
#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
#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
#define GEAR_RATIO 0.0 // gear ratio of motor
#define ENCODER_RES 0.0 // encoder counts per motor shaft revolution
#define WHEEL_DIAMETER 0.0 // diameter of wheel 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
#define MAX_FWD_VEL 0.8 // maximum forwad speed (m/s)
#define MAX_TURN_VEL 2.5 // maximum turning speed (rad/s)
......@@ -49,6 +49,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
......
......@@ -195,13 +195,14 @@ void publish_mb_msgs(){
wheel_ctrl_msg.right_motor_vel_cmd = mb_setpoints.fwd_velocity;
wheel_ctrl_msg.left_motor_vel = mb_state.left_velocity;
wheel_ctrl_msg.right_motor_vel = mb_state.right_velocity;
//TODO: Create Odometry LCM message
//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);
mbot_wheel_ctrl_t_publish(lcm,"MBOT_WHEEL_CTRL",&wheel_ctrl_msg);
mbot_wheel_ctrl_t_publish(lcm,"MBOT_CTRL_COMMAND",&wheel_ctrl_msg);
// odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
}
......@@ -265,6 +266,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;
printf("%f,%f\n",mb_setpoints.fwd_velocity,mb_setpoints.turn_velocity);
}
......
......@@ -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"
......
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