Commit adb25fde authored by pgaskell's avatar pgaskell
Browse files

initial commit

parents
*.a
*.o
lcmtypes/*.c
lcmtypes/*.h
lcmtypes/*.hpp
lcmtypes/*.cpp
bin/*
java/.LCMTYPES_CLASS
java/build/*
java/rob550.jar
java/src/*
optitrack/common/lib/libcommon.a
python/lcmtypes/*
all:
@make -C lcmtypes --no-print-directory
@make -C mobilebot --no-print-directory
@make -C test_motors --no-print-directory
@make -C measure_motor_params --no-print-directory
@make -C python
laptop:
#source setenv.sh
@make -C lcmtypes --no-print-directory
@make -C java --no-print-directory
@make -C python --no-print-directory
clean:
@make -C lcmtypes -s clean
@make -C mobilebot -s clean
@make -C test_motors -s clean
@make -C measure_motor_params -s clean
@make -C java -s clean
@make -C python -s clean
/*******************************************************************************
* Mobilebot Template Code for MBot/MBot-Mini
* pgaskell@umich.edu
*
* This code is a template for completing a fully featured
* mobile robot controller
*
* Functions that need completing are marked with "TODO:"
*
*******************************************************************************/
bin/ : Binaries folder
mobilebot/mobilebot.c/.h : Main setup and threads
test_motors/test_motors.c/.h : Program to test motor implementation
meas..params/meas..params.c/.h: Program to measure motor parameters
common/mb_controller.c/.h : Contoller for manual and autonomous nav
common/mb_defs.h : Define hardware config
common/mb_odometry.c/.h : Odometry and dead reckoning
lcmtypes/ : lcmtypes for Mobilebot
java/ : java build folder for lcmtypes for lcm-spy
setenv.sh : sets up java PATH variables for lcm-spy (run with: source setenv.sh)
#include "../mobilebot/mobilebot.h"
/*******************************************************************************
* int mb_initialize()
*
* this initializes all the PID controllers from the configuration file
* you can use this as is or modify it if you want a different format
*
* return 0 on success
*
*******************************************************************************/
int mb_initialize_controller(){
mb_load_controller_config();
return 0;
}
/*******************************************************************************
* int mb_load_controller_config()
*
* this provides a basic configuration load routine
* you can use this as is or modify it if you want a different format
*
* return 0 on success
*
*******************************************************************************/
int mb_load_controller_config(){
FILE* file = fopen(CFG_PATH, "r");
if (file == NULL){
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
* );
*
******/
fclose(file);
return 0;
}
/*******************************************************************************
* int mb_controller_update()
*
* TODO: Write your PID controller here
* take inputs from the global mb_state
* write outputs to the global mb_state
*
* return 0 on success
*
*******************************************************************************/
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
return 0;
}
/*******************************************************************************
* int mb_destroy_controller()
*
* TODO: Free all resources associated with your controller
*
* return 0 on success
*
*******************************************************************************/
int mb_destroy_controller(){
return 0;
}
#ifndef MB_CONTROLLER_H
#define MB_CONTROLLER_H
#include "../mobilebot/mobilebot.h"
#define CFG_PATH "/home/debian/mobilebot-w20/bin/pid.cfg"
int mb_initialize_controller();
int mb_load_controller_config();
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints);
int mb_destroy_controller();
/************
* Add your PID and other SISO filters here.
* examples:
* rc_filter_t left_wheel_speed_pid;
* rc_filter_t fwd_vel_sp_lpf;
*************/
/***********
* For each PID filter you want to load from settings
* add a pid_parameter_t or filter_parameter_t
* example:
* pid_parameters_t left_wheel_speed_params;
* filter_parameters_t fwd_vel_sp_lpf_params;
************/
#endif
/*******************************************************************************
* mb_defs.h
*
* defines for your bot
* You will need to fill this in based on the data sheets, schematics, etc.
* and your specific configuration...
*
*******************************************************************************/
#ifndef MB_DEFS_H
#define MB_DEFS_H
//#define EXT_CAPE //for use with the MOBILE ROB CAPE VERSIONS BELOW
//#define MRC_VERSION_1v3
//#define MRC_VERSION_2v1
#define BEAGLEBONE_BLUE
#define DEFAULT_PWM_FREQ 25000 // period of motor drive pwm
#define LEFT_MOTOR 1 // id of left motor
#define RIGHT_MOTOR 2 // id of right motor
// TODO: Add convienient defines to define things like motor and encoder polarity here
// 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 MAX_FWD_VEL 0.8 // maximum forwad speed (m/s)
#define MAX_TURN_VEL 2.5 // maximum turning speed (rad/s)
// These rates are set to defaults - can be changed
#define SAMPLE_RATE_HZ 50 // main filter and control loop speed
#define DT 0.02 // 1/sample_rate
#define PRINTF_HZ 10 // rate of print loop
#define RC_CTL_HZ 25 // main filter and control loop speed
#define LCM_HZ 100 // rate of LCM subscribe
#define LCM_PRIORITY 60 // priority of LCM thread (lower is less critical)
#define SETPOINT_PRIORITY 30 // priority of setpoint thread (lower is less critical)
#define CONTROLLER_PRIORITY 90 // priority of controller (lower is less critical)
// LCM Channel Names - should not be changed
#define TRUE_POSE_CHANNEL "TRUE_POSE"
#define ODOMETRY_CHANNEL "ODOMETRY"
#define RESET_ODOMETRY_CHANNEL "RESET_ODOMETRY"
#define CONTROLLER_PATH_CHANNEL "CONTROLLER_PATH"
#define MBOT_IMU_CHANNEL "MBOT_IMU"
#define MBOT_ENCODER_CHANNEL "MBOT_ENCODERS"
#define MBOT_MOTOR_COMMAND_CHANNEL "MBOT_MOTOR_COMMAND"
#define MBOT_TIMESYNC_CHANNEL "MBOT_TIMESYNC"
#define LCM_ADDRESS "udpm://239.255.76.67:7667?ttl=1"
#define LED_OFF 1
#define LED_ON 0
#endif
/*******************************************************************************
* mb_motors.c
*
* Control external Brushed DC Motor Drivers
*
*******************************************************************************/
#include <stdio.h>
#include <rc/motor.h>
#include <rc/model.h>
#include <rc/gpio.h>
#include <rc/pwm.h>
#include <rc/adc.h>
#include "mb_motor.h"
#include "mb_defs.h"
// preposessor macros
#define unlikely(x) __builtin_expect (!!(x), 0)
// global initialized flag
static int init_flag = 0;
/*******************************************************************************
* int mb_motor_init()
*
* initialize mb_motor with default frequency
*******************************************************************************/
int mb_motor_init(){
return mb_motor_init_freq(DEFAULT_PWM_FREQ);
}
/*******************************************************************************
* int mb_motor_init_freq()
* TODO
* set up pwm channels, gpio assignments and make sure motors are left off.
*******************************************************************************/
int mb_motor_init_freq(int pwm_freq_hz){
init_flag = 1;
return 0;
}
/*******************************************************************************
* mb_motor_cleanup()
* TODO
* clean up and release memory after using the motors
*******************************************************************************/
int mb_motor_cleanup(){
if(!init_flag) return 0;
return 0;
}
#ifdef MRC_VERSION_2v1
/*******************************************************************************
* mb_motor_brake()
* TODO
* allows setting the brake function on the motor drivers
* returns 0 on success, -1 on failure
*******************************************************************************/
int mb_motor_brake(int brake_en){
if(unlikely(!init_flag)){
fprintf(stderr,"ERROR: trying to enable brake before motors have been initialized\n");
return -1;
}
return 0
}
#endif
/*******************************************************************************
* int mb_disable_motors()
* TODO
* disables PWM output signals
* returns 0 on success
*******************************************************************************/
int mb_motor_disable(){
if(unlikely(!init_flag)){
fprintf(stderr,"ERROR: trying to disable motors before motors have been initialized\n");
return -1;
}
return 0;
}
/*******************************************************************************
* int mb_motor_set(int motor, double duty)
* TODO
* set a motor direction and power
* motor is from 1 to N, duty is from -1.0 to +1.0
* use defines in mb_defs.h
* returns 0 on success
*******************************************************************************/
int mb_motor_set(int motor, double duty){
if(unlikely(!init_flag)){
fprintf(stderr,"ERROR: trying to rc_set_motor_all before they have been initialized\n");
return -1;
}
return 0;
}
/*******************************************************************************
* int mb_motor_set_all(double duty)
* TODO
* applies the same duty cycle argument to both motors
*******************************************************************************/
int mb_motor_set_all(double duty){
if(unlikely(!init_flag)){
printf("ERROR: trying to rc_set_motor_all before they have been initialized\n");
return -1;
}
return 0;
}
#ifdef MRC_VERSION_2v1
/*******************************************************************************
* int mb_motor_read_current(int motor)
* TODO
* returns the measured current in A
*******************************************************************************/
double mb_motor_read_current(int motor){
return 0.0;
}
#endif
\ No newline at end of file
/*******************************************************************************
* mb_motor.h
*******************************************************************************/
#ifndef MB_MOTOR_H
#define MB_MOTOR_H
//fuctions
int mb_motor_init();
int mb_motor_init_freq(int pwm_freq_hz);
int mb_motor_disable();
int mb_motor_brake(int brake_en);
int mb_motor_set(int motor, double duty);
int mb_motor_set_all(double duty);
int mb_motor_cleanup();
double mb_motor_read_current(int motor);
#endif
\ No newline at end of file
/*******************************************************************************
* mb_odometry.c
*
* TODO: Implement these functions to add odometry and dead rekoning
*
*******************************************************************************/
#include "../mobilebot/mobilebot.h"
#include "mb_defs.h"
#include <math.h>
#define PI 3.14159265358979323846
/*******************************************************************************
* mb_initialize_odometry()
*
* TODO: initialize odometry
* NOTE: you should initialize from Optitrack data if available
*
*******************************************************************************/
void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float theta){
}
/*******************************************************************************
* mb_update_odometry()
*
* TODO: calculate odometry from internal variables
* publish new odometry to lcm ODOMETRY_CHANNEL
*
*******************************************************************************/
void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
}
/*******************************************************************************
* mb_clamp_radians()
* clamp an angle from -PI to PI
*******************************************************************************/
float mb_clamp_radians(float angle){
if(angle < -PI)
{
for(; angle < -PI; angle += 2.0*PI);
}
else if(angle > PI)
{
for(; angle > PI; angle -= 2.0*PI);
}
return angle;
}
/*******************************************************************************
* mb_angle_diff_radians()
* computes difference between 2 angles and wraps from -PI to PI
*******************************************************************************/
float mb_angle_diff_radians(float angle1, float angle2){
float diff = angle2 - angle1;
while(diff < -PI) diff+=2.0*PI;
while(diff > PI) diff-=2.0*PI;
return diff;
}
\ No newline at end of file
/*******************************************************************************
* mb_odometry.h
*
*
*******************************************************************************/
#ifndef MB_ODOMETRY_H
#define MB_ODOMETRY_H
#include "mb_structs.h"
void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float theta);
void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state);
float mb_clamp_radians(float angle);
float mb_angle_diff_radians(float angle1, float angle2);
#endif
\ No newline at end of file
#ifndef MB_STRUCTS_H
#define MB_STRUCTS_H
#include <inttypes.h>
typedef struct mb_state mb_state_t;
struct mb_state{
// raw sensor inputs
float tb_angles[3]; // DMP filtered angles, tb_angles[3] is heading
float accel[3]; // units of m/s^2
float gyro[3]; // units of degrees/s
float mag[3]; // units of uT
float temp; // units of degrees Celsius
float last_yaw;
int left_encoder_delta; // left encoder counts since last reading
int right_encoder_delta; // right encoder counts since last reading
uint64_t left_encoder_total; //total encoder ticks since running
uint64_t right_encoder_total;
float fwd_velocity;
float turn_velocity;
float left_velocity;
float right_velocity;
float opti_x; // Optitrack coordinates
float opti_y; // (if using optitrack for ground truth)
float opti_theta; // Optitrack heading
//outputs
float left_cmd; //left wheel command [-1..1]
float right_cmd; //right wheel command [-1..1]
//TODO: Add more variables to this state as needed
};
typedef struct mb_setpoints mb_setpoints_t;
struct mb_setpoints{
float fwd_velocity; // fwd velocity in m/s
float turn_velocity; // turn velocity in rad/s
int manual_ctl;
};
typedef struct mb_odometry mb_odometry_t;
struct mb_odometry{
float x; //x position from initialization in m
float y; //y position from initialization in m
float theta; //orientation from initialization in rad
};
typedef struct pid_parameters pid_parameters_t;
struct pid_parameters {
float kp;
float ki;
float kd;
float dFilterHz;
float out_lim;
float int_lim;
};
#endif
\ No newline at end of file
LCM_JAR = /usr/local/share/java/lcm.jar
ROB550_JAR = rob550.jar
BUILD_PATH = build
SOURCE_PATH = src
# Construct the pathnames to the C types
LCMTYPES_NAMES := $(basename $(shell cd ../lcmtypes && ls *.lcm))
LCMTYPES_JAVA := $(addsuffix .java, $(addprefix $(SOURCE_PATH)/rob550/lcmtypes/, $(LCMTYPES_NAMES)))
LCMTYPES_CLASS := $(addsuffix .class, $(addprefix $(BUILD_PATH)/rob550/lcmtypes/, $(LCMTYPES_NAMES)))
all: $(ROB550_JAR)
src/rob550/lcmtypes/%.java: ../lcmtypes/%.lcm
@echo "\t$<"
@lcm-gen -j --jpath=src --jdefaultpkg=rob550.lcmtypes $<
.LCMTYPES_CLASS: $(BUILD_PATH) $(LCMTYPES_JAVA)
@echo "[java/lcmtypes]"
@echo "\t$(LCMTYPES_JAVA)"
@javac -classpath $(LCM_JAR) -d $(BUILD_PATH) $(LCMTYPES_JAVA)
@touch .LCMTYPES_CLASS
$(ROB550_JAR): $(BUILD_PATH) .LCMTYPES_CLASS
@echo "[java/rob550.jar]"
@echo "$(ROB550_JAR)"
@jar cf $(ROB550_JAR) -C $(BUILD_PATH) .
$(BUILD_PATH):
@mkdir -p $(BUILD_PATH)
clean:
@echo "clean [java/lcmtypes]"; rm -f $(SOURCE_PATH)/rob550/lcmtypes/*
@echo "clean [java/build]"; rm -rf build/*
@echo "clean [java/rob550.jar]"; rm -f $(ROB550_JAR)
@rm -f *~ .LCMTYPES_CLASS
LCM = lcm-gen
LCMTYPES_NAMES := $(shell ls *.lcm)
# Construct the pathnames to the C types
LCMTYPES_C := $(LCMTYPES_NAMES:%.lcm=%.c)
LCMTYPES_O = $(LCMTYPES_C:%.c=%.o)
ALL = $(LCMTYPES_C) $(LCMTYPES_O)
CC = gcc
CFLAGS = -g `pkg-config --cflags lcm`
all: $(ALL)
%.c: %.lcm
$(LCM) -c --c-typeinfo $<
$(LCM) -x $<
%.o: %.c
$(CC) $(CFLAGS) -c $^ -o $@
clean:
rm -rf build/
rm -f *.c *.h *.o *.cpp *.hpp *~
struct mbot_encoder_t
{
int64_t utime;
int64_t leftticks;
int64_t rightticks;
int16_t left_delta;
int16_t right_delta;
}
\ No newline at end of file
struct mbot_imu_t
{
int64_t utime;
float gyro[3];
float accel[3];
float mag[3];
float tb_angles[3];
float temp;
}
\ No newline at end of file
struct mbot_motor_command_t
{
int64_t utime;
float trans_v; // in m/s
float angular_v; // in rad/s
}
// Receive Confirmation of a particular lcm message
struct message_recieved_t
{
int64_t utime; // Time of confirmation message creation
int64_t creation_time; // time of message creation (assumption that this will be unique between messages)
string channel; //name of channel
}
struct odometry_t
{
int64_t utime;
float x;
float y;
float theta;
}
struct oled_message_t
{
int64_t utime;