initial commit

.gitignore

+*.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/*

Makefile

+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

README.txt

+/*******************************************************************************
+*                 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)

common/mb_controller.c

+#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;
+}

common/mb_controller.h

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

common/mb_defs.h

+/*******************************************************************************
+* 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

common/mb_motor.c

+/*******************************************************************************
+* 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

common/mb_motor.h

+/*******************************************************************************
+* 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

common/mb_odometry.c

+/*******************************************************************************
+* 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

common/mb_odometry.h

+/*******************************************************************************
+* 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

common/mb_structs.h

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

java/Makefile

+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

lcmtypes/Makefile

+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 *~

lcmtypes/mbot_encoder_t.lcm

+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

lcmtypes/mbot_imu_t.lcm

+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

lcmtypes/mbot_motor_command_t.lcm

+struct mbot_motor_command_t
+{
+
+    int64_t utime;
+
+    float trans_v; // in m/s
+    float angular_v; // in rad/s
+}

lcmtypes/message_recieved_t.lcm

+// 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 
+}

lcmtypes/odometry_t.lcm

+struct odometry_t
+{
+    int64_t utime;
+
+    float x;
+    float y;
+    float theta;
+
+}

lcmtypes/oled_message_t.lcm

+struct oled_message_t
+{
+    int64_t utime;