Commit 6a23b8d9 authored by Demo User's avatar Demo User
Browse files

1.2

parent 6797fe0b
......@@ -9,9 +9,34 @@
* return 0 on success
*
*******************************************************************************/
struct pid_parameters pid_params;
/*
Within mb_controller.c, create two controllers,
1. control each wheel velocity using your open loop calibration
2. control each wheel’s speed using PID controllers.
The controllers should keep the wheels moving the desired speed
so the robot moves in as straight a line as possible without
heading correction but likely this won’t be perfect.
These controllers will be a starting point for a more sophisticated controller.
You can modify the python scripts 'drive_test.py' and 'drive_square.py'
to generate velocity commands to test your controller.
*/
int mb_initialize_controller(){
mb_load_controller_config();
left_vel_pid_filter = rc_filter_empty();
right_vel_pid_filter = rc_filter_empty();
rc_filter_pid(&left_vel_pid_filter, pid_params.kp, pid_params.ki, pid_params.kd, 0.1,DT );
rc_filter_pid(&right_vel_pid_filter, pid_params.kp, pid_params.ki, pid_params.kd, 0.1,DT );
rc_filter_enable_saturation(&left_vel_pid_filter, -1.0, 1.0);
rc_filter_enable_saturation(&right_vel_pid_filter, -1.0, 1.0);
return 0;
}
......@@ -32,18 +57,28 @@ int mb_load_controller_config(){
printf("Error opening pid.cfg\n");
}
/******
*
* Example of loading a line from .cfg file:
*
* fscanf(file, "%f %f %f %f",
* &pid_params.kp,
* &pid_params.ki,
* &pid_params.kd,
* &pid_params.dFilterHz
* );
*
******/
/*
*****
*
* Example of loading a line from .cfg file:
*
* fscanf(file, "%f %f %f %f",
* &pid_params.kp,
* &pid_params.ki,
* &pid_params.kd,
* &pid_params.dFilterHz
* );
*
*****
*/
// pid_params = malloc(sizeof(pid_parameters));
fscanf(file, "kp: %f\nki: %f\nkd: %f\ndFilterHz: %f",
&pid_params.kp,
&pid_params.ki,
&pid_params.kd,
&pid_params.dFilterHz
);
fclose(file);
return 0;
......@@ -60,7 +95,107 @@ int mb_load_controller_config(){
*
*******************************************************************************/
// float Kp, Ki, Kd;
// float Kp = pid_params.kp;
// float Kd = pid_params.kd;
// float Ki = pid_params.ki;
/* PID algorithm:
// x : left / right velocity
// xdot: left/right acc
// float Kp, Kd, Ki;
// float left_up, left_ui, left_ud, right_up, right_ui, right_ud;
// float left_speed, right_speed;
// float left_acc=0, right_acc=0, left_acc_last=0, right_acc_last=0;
// float left_ui_last, right_ui_last;
// float left_speed_last, right_speed_last;
// float left_cmd, right_cmd;
// float left_err, left_err_d, left_err_i;
// float right_err, right_err_d, right_err_i;
// float left_last_err, right_last_err;
*/
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
// simply march it!
float left_error = mb_setpoints->fwd_velocity - mb_state->left_velocity;
float right_error = mb_setpoints->fwd_velocity - mb_state->right_velocity;
mb_state->left_cmd = rc_filter_march( &left_vel_pid_filter, left_error);
mb_state->right_cmd = rc_filter_march( &right_vel_pid_filter, right_error);
/*
Kp = pid_params.kp;
Kd = pid_params.kd;
Ki = pid_params.ki;
// int dt = time_offset;
int dt = 1;
// 1. Convert setpoints linear.x & angular.z to left/right speed.
float x = mb_setpoints->fwd_velocity; // linear x
float z = mb_setpoints->turn_velocity; // angular z
// Compute Setpoints
float left_setpoint = (x- z*WHEEL_BASE/2) / (WHEEL_DIAMETER/2);
float right_setpoint = (x+ z*WHEEL_BASE/2) / (WHEEL_DIAMETER/2);
// 2. Get current speed (state)
left_speed = mb_state->left_velocity;
right_speed = mb_state -> right_velocity;
// Compute 'speed' error.
left_err = left_setpoint - left_speed;
right_err = right_setpoint - right_speed;
// 3. Compute x_dot (acceleration)
// x_dot = (x - x_k-1 ) / dt
left_acc = (left_speed - left_speed_last)/dt;
right_acc = (right_speed - right_speed_last)/dt;
// 4. Compute up, ui and ud
// up = Kp * err
left_up = Kp * left_err;
right_up = Kp * right_err;
// ui = ui_k-1 + Ki*(err)
left_ui = left_ui_last + Ki * left_err;
right_ui = right_ui_last + Ki * right_err;
// ud = Kd* ( x_dot - x_dotk-1)
left_ud = Kd * (left_acc - left_acc_last);
right_ud = Kd * (right_acc - right_acc_last);
// U = up + ui + ud
left_cmd = left_up + left_ui + left_ud;
right_cmd = right_up + right_ui + right_ud;
// 5. Send commad ! (Write state.)
// Print command out first
// printf("left_cmd right_cmd: (%.3f, %.3f)\r", left_cmd, right_cmd);
mb_state->right_cmd = right_cmd;
mb_state->left_cmd = left_cmd;
///////////// 6. UPDATE /////////////
// update 'last' speed for next-iter left/right acc
left_speed_last = left_speed;
right_speed_last = right_speed;
// update 'last' ui for next-iter ui
left_ui_last = left_ui;
right_ui_last = right_ui;
// update 'last' acc for next-iter ud
left_acc_last = left_acc;
right_acc_last = right_acc;
// // update last_err
// left_last_err = left_err;
// right_last_err = right_err;
*/
return 0;
}
......@@ -75,5 +210,8 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
*******************************************************************************/
int mb_destroy_controller(){
// Free filters
return 0;
}
......@@ -3,7 +3,8 @@
#include "../mobilebot/mobilebot.h"
#define CFG_PATH "/home/debian/mobilebot-w20/bin/pid.cfg"
// #define CFG_PATH "/home/debian/mobilebot-w20/bin/pid.cfg"
#define CFG_PATH "/home/debian/mobilebot/common/pid_params.yaml" //pid.cfg"
int mb_initialize_controller();
int mb_load_controller_config();
......
......@@ -18,14 +18,18 @@
#define RIGHT_MOTOR 2 // id of right motor
// TODO: Add convienient defines to define things like motor and encoder polarity here
#define LEFT_POLARITY -1
#define RIGHT_POLARITY 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 WHEEL_BASE 0.0 // wheel separation distance in meters
#define GEAR_RATIO 78.0 // gear ratio of motor
#define ENCODER_RES 1560.0 // encoder counts per motor shaft revolution
#define WHEEL_DIAMETER 0.083 // diameter of wheel in meters
#define WHEEL_BASE 0.18 // 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)
// #define CFG_PATH "../pid_params.yaml"
// These rates are set to defaults - can be changed
#define SAMPLE_RATE_HZ 50 // main filter and control loop speed
......
kp: 0.3
ki: 0.01
kd: 0.01
dFilterHz: 0.1
\ No newline at end of file
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
0.000000,0.000000
0.000000,0.030288
0.067182,0.085065
0.120186,0.140163
0.173029,0.195101
0.227322,0.250039
0.282743,0.305137
0.337036,0.359914
0.391330,0.414207
0.447234,0.469306
0.502333,0.523599
0.557431,0.580309
0.612691,0.634441
0.666662,0.686478
0.725466,0.734972
0.778471,0.785398
0.840175,0.838886
0.888668,0.893340
0.938450,0.949728
0.992099,1.011593
1.063792,1.067175
......@@ -11,7 +11,7 @@ SOURCES := $(SOURCES) $(wildcard ../common/*.c)
INCLUDES := $(wildcard *.h)
INCLUDES := $(INCLUDES) $(wildcard ../common/*.h)
OBJECTS := $(SOURCES:$%.c=$%.o)
# Note: .c / .cpp => link libraries: .o (object files) => binary files.(executables)
prefix := /usr/local
RM := rm -f
INSTALL := install -m 4755
......@@ -32,6 +32,7 @@ $(OBJECTS): %.o : %.c $(INCLUDES)
@$(CC) $(CFLAGS) -c $< -o $(@)
@echo "Compiled: "$<
# make all is the same as make
all:
$(TARGET)
......
......@@ -24,14 +24,35 @@
float enc2meters = (WHEEL_DIAMETER * M_PI) / (GEAR_RATIO * ENCODER_RES);
void test_speed(float du, float dtime_s);
void test_speed(int motor_ch, float du, float dtime_s);
/*******************************************************************************
* int main()
*
*******************************************************************************/
int main(){
// #define GEAR_RATIO 78
#define ENC_CNTS 20
#define PI 3.14159265358979323846
// #define WHEEL_DIAMETER 0.083 // 8.33 cm
// Global speed numpy arrays
float speed_left[21];
float speed_right[21];
void Write_Output_File(float* speed_left, float* speed_right, char* filename);
int main(int argc, char** argv){
if(argc <2){
printf("Wrong input format.\n");
printf("Type: sudo ./measure_motors <output_filename.txt>\n");
return 0;
}
// make sure another instance isn't running
// if return value is -3 then a background process is running with
// higher privaledges and we couldn't kill it, in which case we should
......@@ -78,11 +99,131 @@ int main(){
}
// TODO: Plase exit routine here
printf("Ready to Calibrate...\n");
printf("Calibrating Left Motor...\n");
test_speed(1, 0.5, 0.5);
printf("Calibrating Right Motor...\n");
test_speed(2, 0.5, 0.5);
printf("Done Calibration.\n");
// char filename[30] = "1.1.txt";
// char filename[30] = argv[1];
Write_Output_File(speed_left, speed_right, argv[1]);
printf("\n ==== Profiling Results: =======\n");
for(int i=0; i<=20; i++){
printf("PWM: %f, Left: %f, Right: %f\n", i*0.05, speed_left[i] ,speed_right[i]);
}
// 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 Write_Output_File(float* speed_left, float* speed_right, char* filename){
FILE *f = fopen(filename, "w");
if (f == NULL)
{
printf("Error opening file!\n");
exit(1);
}
/* print some text */
// const char *text = "Duty Cycles v.s. Motor Speed (m/s)";
// fprintf(f, "Some text: %s\n", text);
// /* print integers and floats */
// int i = 1;
// float pi= 3.1415927;
// fprintf(f, "Integer: %d, float: %f\n", i, pi);
for(int i=0; i<21; i++){
fprintf(f,"%f,%f\n",speed_left[i], speed_right[i]);
// fprintf(f, "Duty cycle: %f, Left: %f, Right %f\n", i*0.05, speed_left[i], speed_right[i]);
}
/* printing single chatacters */
// char c = 'A';
// fprintf(f, "A character: %c\n", c);
fclose(f);
}
// Call every second
float ticks_to_speed(float number_of_ticks){
float speed = 0;
float wheel_rps = 0;
float mps = 0;
speed = abs(number_of_ticks); // counts per second
wheel_rps = speed / (GEAR_RATIO*ENC_CNTS); // wheel revs per second
mps = wheel_rps * WHEEL_DIAMETER * PI; // (m/s) or the robot (circumference = 2*PI*r)
return mps;
// return speed;
}
void test_speed(int motor_ch, float duty, float dtime_s){
// run duty cycle 'duty' with time dtime_s
int index = 0;
rc_encoder_eqep_init();
rc_motor_init();
for (float pwm=0.0; pwm<= 1.1; pwm += 0.05){
rc_motor_set(motor_ch, pwm);
rc_nanosleep(2E8);
rc_encoder_eqep_write(motor_ch, 0); // reset encoder counts
rc_nanosleep(1E9); // sleep 1 second.
int ticks = rc_encoder_eqep_read(motor_ch); // read channel one
float speed = ticks_to_speed(ticks);
if(motor_ch == 1){
// left
speed_left[index++] = speed;
}
else{
speed_right[index++] = speed;
}
printf("Duty Cycle: %f, Speed: %f (m/s)\n", pwm, speed);
// printf("Duty Cycle: %f, Speed: %f (counts/second)\n", pwm, speed);
}
rc_motor_set(motor_ch, 0.0);
rc_encoder_eqep_cleanup();
rc_motor_cleanup();
// record these data into a numpy array (a one-d float array)
}
/*
TODO:
1. Check gear ratio: test encoder program : 1635/78~20.96 =>
- One revolution for encoder: 20 counts
- 20 * 78 = 1560 counts per wheel revolution!
2. Output .txt file and use python or excel to plot curve
*/
/*
measure the 'loaded steady state wheel speed' vs. 'input PWM'.
You will likely want to have the program output data for analysis with 'numpy'.
You will need to f'ill in parameters of the robot in the file common/mb_defs.h '
which includes compiler #defines for parameters like
(1) wheel diameter
(2) gear ratio
(3) encoder resolution
then use these in your code.
You can find these values by consulting the MBot-Mini documentation or
by measuring parameters in the lab.
- Report - :
1. motor speed vs. pwm
Q:
How much variation is there in the calibration functions?
What do you think is the source of variation?
*/
\ No newline at end of file
......@@ -85,8 +85,10 @@ int main(){
rc_encoder_init();
rc_encoder_write(1, 0);
rc_encoder_write(2, 0);
// mb_odometry.c
mb_initialize_odometry(&mb_odometry, 0.0,0.0,0.0);
// This is the main function for mb_controller.c
printf("attaching imu interupt...\n");
rc_mpu_set_dmp_callback(&mobilebot_controller);
......@@ -128,6 +130,21 @@ int main(){
* TODO: modify this function to read other sensors
*
*******************************************************************************/
#define ENC_CNTS 20
#define PI 3.14159
// Call every 'time_offset' KEY : time_offset DT is float (0.2)
float ticks_to_speed(int number_of_ticks, float time_offset){
float speed = 0;
float wheel_rps = 0;
float mps = 0;
speed = number_of_ticks / time_offset; // ( sampling perios // ticks per second
wheel_rps = speed / (GEAR_RATIO*ENC_CNTS); // wheel revs per second
mps = wheel_rps * WHEEL_DIAMETER * PI; // (m/s) or the robot (circumference = 2*PI*r)
return mps;
// return speed; (m/s)
}
void read_mb_sensors(){
pthread_mutex_lock(&state_mutex);
// Read IMU
......@@ -151,7 +168,10 @@ 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);
/////// Calculate mb_state.left_velocity here (floating pts error)
mb_state.left_velocity = ticks_to_speed(mb_state.left_encoder_delta, DT);
mb_state.right_velocity = ticks_to_speed(mb_state.right_encoder_delta, DT);
//unlock state mutex
pthread_mutex_unlock(&state_mutex);
......@@ -166,7 +186,14 @@ void read_mb_sensors(){
void publish_mb_msgs(){
mbot_imu_t imu_msg;
mbot_encoder_t encoder_msg;
// odometry_t odo_msg;
odometry_t odo_msg;
mbot_wheel_ctrl_t wheel_ctrl_msg;
// mbot_motor_command_t motor_cmd_msg;
/*
float x; //x position from initialization in m
float y; //y position from initialization in m
float theta;
*/
//Create IMU LCM Message
imu_msg.utime = now;
......@@ -186,11 +213,28 @@ void publish_mb_msgs(){
encoder_msg.rightticks = mb_state.right_encoder_total;
//TODO: Create Odometry LCM message
odo_msg.utime = now;
odo_msg.x = mb_state.opti_x;
odo_msg.y = mb_state.opti_y;
odo_msg.theta = mb_state.opti_theta;
// Publish motor msgs
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.left_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 = mb_state.left_velocity;
wheel_ctrl_msg.right_motor_vel = mb_state.right_velocity;
//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);
// odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
mbot_wheel_ctrl_t_publish(lcm, "MBOT_WHEEL_CTRL_CHANNEL", &wheel_ctrl_msg);
// mbot_motor_command_t_publish(lcm, MBOT_MOTOR_COMMAND_CHANNEL, &wheel_ctrl_msg);
}
/*******************************************************************************
......@@ -206,11 +250,22 @@ void publish_mb_msgs(){
*
*
*******************************************************************************/
#define left_channel 1
#define right_channel 2
void mobilebot_controller(){
update_now();
// get mb_state.left/right velocity from read_mb_sensors();
read_mb_sensors();
// Controller Update Commands
mb_controller_update(&mb_state, &mb_setpoints);
// Drive motors
rc_motor_set(LEFT_MOTOR, LEFT_POLARITY * mb_state.left_cmd );
rc_motor_set(RIGHT_MOTOR, RIGHT_POLARITY * mb_state.right_cmd);
// publish some info (odometry) to RPi
publish_mb_msgs();
}
......@@ -241,7 +296,7 @@ void timesync_handler(const lcm_recv_buf_t * rbuf, const char *channel,
/*******************************************************************************
* motor_command_handler()
* motor_command_handler()
*
* sets motor velocity setpoints from incoming lcm message
*
......
......@@ -36,6 +36,8 @@
#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"
......@@ -49,6 +51,9 @@ rc_mpu_data_t imu_data;
pthread_mutex_t state_mutex;
mb_state_t mb_state;
mb_setpoints_t mb_setpoints;
rc_filter_t left_vel_pid_filter;
rc_filter_t right_vel_pid_filter;
mb_odometry_t mb_odometry;
int64_t now;
int64_t time_offset;
......
#! /usr/bin/python
import lcm
from time import sleep
import sys
sys.path.append("lcmtypes")
from lcmtypes import mbot_encoder_t
from lcmtypes import mbot_imu_t
from lcmtypes import mbot_motor_command_t
from lcmtypes import odometry_t
from lcmtypes import pose_xyt_t
from lcmtypes import robot_path_t
from lcmtypes import timestamp_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 #go 0.25m in 1s
drive_command.angular_v = 0.0
turn_command = mbot_motor_command_t()
turn_command.trans_v = 0.0
turn_command.angular_v = 3.1415/2.0 #turn 180 in 2s
lc.publish("MBOT_MOTOR_COMMAND",drive_command.encode())
sleep(2.0)
# lc.publish("MBOT_MOTOR_COMMAND",turn_command.encode())
# sleep(1.0)
# lc.publish("MBOT_MOTOR_COMMAND",drive_command.encode())
# sleep(2.0)
# lc.publish("MBOT_MOTOR_COMMAND",turn_command.encode())