main.cpp

00001 //Balance Bot V4
00002 //Author: Carter Sharer
00003 //Date: 10/13/2016
00004 //ESE519 Lab 6 Part 3 Skeleton Code
00005 
00006 /******************************* README USAGE *******************************
00007 * This robot must be powered on while it is laying down flat on a still table
00008 * This allows the robot to calibrate the IMU (~5 seconds)
00009 * The motors are DISABLED when the robot tilts more then +-45 degrees from
00010 * vertical.  To ENABLE the motors you must lift the robot to < +- 45 degres and
00011 * press the joystick button.
00012 * To reset the motor positions you must press the josystick button anytime.
00013 ******************************************************************************/
00014 
00015 //Balance Bot Begin
00016 #include "mbed.h"
00017 #include "pin_assignments.h"
00018 #include "balance_bot.h"
00019 #include "stepper_motors.h"
00020 #include "MRF24J40.h"
00021 
00022 #define WHEEL_BASE 100 //mm
00023 #define NUM_STEPS 1600 //number of steps for 1 turn of wheel
00024 #define WHEEL_RADIUS 51 //units in mm
00025 #define PI 3.1415
00026 #define maxAccel 5
00027 //#define CIRC 2*PI*WHEEL_RADIUS
00028 
00029 //For RF Communication
00030 #define JSTICK_H 8
00031 #define JSTICK_V 9
00032 #define SPACE 10
00033 #define KNOB1 11
00034 #define KNOB2 12
00035 #define KNOB3 13
00036 #define KNOB4 14
00037 #define BUTTON 16
00038 #define JSTICK_OFFSET 100
00039 #define TX_BUFFER_LEN 18
00040 #define TX_ANGLE_OFFSET 100
00041 //Knobs
00042 #define POT1 p17
00043 #define POT2 p18
00044 #define POT3 p16
00045 #define POT4 p15
00046 //JoyStick
00047 #define POTV p19
00048 #define POTH p20
00049 #define X 500
00050 #define Y 0
00051 
00052 
00053 //PID
00054 #define MAX_THROTTLE 100
00055 #define MAX_TARGET_ANGLE 12
00056 //PID Default control values from constant definitions
00057 float Kp1;
00058 float Kd1;
00059 float Kp2;
00060 float Kd2;
00061 int i;
00062 
00063 //Controller Values
00064 uint8_t knob1, knob2, knob3, knob4;
00065 int8_t jstick_h, jstick_v;
00066 
00067 //Control Variables
00068 float throttle = 0; //From joystick
00069 float steering = 0; //From joystick
00070 int robot_pos = 0; //Robots position
00071 float motor1_old = 0; 
00072 
00073 Timer timer;
00074 int timer_value;
00075 int timer_old;
00076 int dt;
00077 
00078 //Loop Counters
00079 uint8_t slow_loop_counter;
00080 uint8_t medium_loop_counter;
00081 uint8_t loop_counter;
00082 
00083 Serial pc(USBTX, USBRX);
00084 
00085 // LEDs
00086 DigitalOut led1(LED1);
00087 DigitalOut led2(LED2);
00088 DigitalOut led3(LED3);
00089 DigitalOut led4(LED4);
00090 
00091 //Button
00092 bool button;
00093 #include "communication.h"
00094 
00095 //Waypoint
00096 //[(10m, 0th). (5m, 90th)]
00097 //(10x, 0y), (x, y)]
00098 
00099 // ================================================================
00100 // ===                      INITIAL SETUP                       ===
00101 // ================================================================
00102 
00103 // ================================================================
00104 // ===                    MAIN PROGRAM LOOP                     ===
00105 // ================================================================
00106 int main()
00107 {
00108     //Used to toggle motors on and off
00109     bool ROBOT_ON = false;
00110 
00111     //Set the Channel. 0 is default, 15 is max
00112     uint8_t channel = 9;
00113     mrf.SetChannel(channel);
00114 
00115     //Used for button press
00116     int last_button_time = 0;
00117     float wheel1_dist = 0;
00118     float wheel2_dist = 0;
00119     float robot_theta = 0;
00120     float distance_traveled = 0;
00121     float world_x = 0;
00122     float world_y = 0;
00123     float robot_pos_old = 0;
00124 
00125     pc.baud(115200);
00126     pc.printf("Start\r\n");
00127     timer.start();
00128     //timer
00129     timer_value = timer.read_us();
00130 
00131     //Init Stepper Motors
00132     //Attach Timer Interupts (Tiker)
00133     timer_M1.attach_us(&ISR1, ZERO_SPEED);
00134     timer_M2.attach_us(&ISR2, ZERO_SPEED);
00135     step_M1 = 1;
00136     dir_M1 = 1;
00137     enable = DISABLE; //Disable Motors
00138 
00139     //Enable Motors
00140     enable = ENABLE;
00141 
00142     while(1) {
00143         //Led 4 to indicate if robot is Running or Not
00144         led4 = ROBOT_ON;
00145 
00146         //Led 2 to indicate a button press
00147         led2 = button;
00148 
00149         //If button is pressed reset motor position
00150         if(button) {
00151             pos_M1 = 0; //Reset position of Motor 1
00152             pos_M2 = 0; //Reset position of motor 2
00153 
00154             //Read Button Press to Toggle Motors
00155             if((timer.read_us() - last_button_time) > 250000) {
00156                 ROBOT_ON = ROBOT_ON^1;
00157                 pc.printf("BUTTON WAS PRESSED \r\n");
00158                 last_button_time = timer.read_us();
00159             }
00160         }
00161 
00162         //Conver Motor Position from Steps to Distance
00163         wheel1_dist = 2*PI*WHEEL_RADIUS* pos_M1/NUM_STEPS;
00164         wheel2_dist = 2*PI*WHEEL_RADIUS* pos_M2/NUM_STEPS;
00165         robot_pos = (wheel1_dist + wheel2_dist) / 2; ///total dist traveled
00166         robot_theta = (wheel1_dist - wheel2_dist) /WHEEL_BASE; //angle
00167 
00168         distance_traveled = robot_pos - robot_pos_old;
00169     
00170     
00171         //Curr Robot X and Y
00172         world_x += distance_traveled * sin(robot_theta); //import sin
00173         world_y += distance_traveled * cos(robot_theta);
00174         
00175         //Pose(x,y,th) = P(world_x, world_y, robot_th)
00176 
00177         pc.printf("X axis: %d",world_x);
00178         pc.printf("Y axis: %d \n",world_y);
00179         pc.printf("robot_pos: %d \n",robot_pos);
00180         //Timer
00181         timer_value = timer.read_us();
00182 
00183         //Joystick control
00184         throttle = jstick_v;
00185         steering = jstick_h;
00186 
00187         /**** Update Values DO NOT MODIFY ********/
00188         loop_counter++;
00189         slow_loop_counter++;
00190         medium_loop_counter++;
00191         dt = (timer_value - timer_old);
00192         timer_old = timer_value;
00193         robot_pos_old = robot_pos;
00194         /*****************************************/
00195 
00196         //Running: Motor Control Enabled
00197         if(ROBOT_ON) {
00198             //TEMPWAYPOINTS
00199             //Get Waypoint
00200             //i = 0;
00201             //X = WAYPOINS[i][0]
00202             //Y = WAYPOINS[i][1]
00203             //calc target theta
00204             float target_theta = 0;
00205             float target_distance = 5000;
00206             //if(robot is at waypoint)
00207             //get new waypoint
00208             //i++
00209             //else {
00210                 //}
00211             
00212             
00213             
00214             //Enable Motor
00215             enable = ENABLE;
00216 
00217             //User Control ONLY 
00218             float theta_error = target_theta - robot_theta;
00219 
00220             if(theta_error > 1 && theta_error < -1) {
00221                 //Angular Controller
00222                 float turn_speed = theta_error*0.5;
00223                 motor1 = turn_speed;
00224                 motor2 = -turn_speed; 
00225             } else {
00226                 //Dist Controller
00227                 float dist_error = target_distance - robot_pos;
00228                 float speed = dist_error*0.5;
00229                 motor1 = speed;
00230                 motor2 = speed;
00231             }
00232             
00233             
00234             //Acceleratoin Cap
00235             float motor1Acel = motor1 - motor1_old;
00236             
00237             if(motor1Acel > maxAccel) { //macAcel = 5
00238                 motor1 = maxAccel;
00239             }
00240 
00241             //Cap the max and min values [-100, 100]
00242             motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
00243             motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
00244             motor1_old = motor1;
00245 
00246             //Set Motor Speed here
00247             setMotor1Speed(motor1);
00248             setMotor2Speed(motor2);
00249 
00250         }
00251         //Robot is off so disable the motors
00252         else {
00253             enable = DISABLE;
00254         }
00255 
00256         /* Here are some loops that trigger at different intervals, this
00257         * will allow you to do things at a slower rate, thus saving speed
00258         * it is important to keep this fast so we dont miss IMU readings */
00259 
00260         //Fast Loop: Good for printing to serial monitor
00261         if(loop_counter >= 5) {
00262             loop_counter = 0;
00263             pc.printf("ROBOT_ON:%d pos_M1:%d pos_M2:%d robot_pos%d \r\n", ROBOT_ON, pos_M1, pos_M2, robot_pos);
00264         }
00265 
00266         //Meduim Loop: Good for sending and receiving
00267         if (medium_loop_counter >= 10) {
00268             medium_loop_counter = 0; // Read  status
00269 
00270             //Recieve Data
00271             rxLen = rf_receive(rxBuffer, 128);
00272             if(rxLen > 0) {
00273                 led1 = led1^1;
00274                 //Process data with our protocal
00275                 communication_protocal(rxLen);
00276             }
00277 
00278         }  // End of medium loop
00279 
00280         //Slow Loop: Good for sending if speed is not an issue
00281         if(slow_loop_counter >= 99) {
00282             slow_loop_counter = 0;
00283 
00284             /* Send Data To Controller goes here *
00285              *                                   */
00286         } //End of Slow Loop
00287 
00288 
00289 
00290     } //end main loop
00291 } //End Main()