Britney Dorval / Mbed 2 deprecated Yusheng-final_project_robot

Code for robot

Dependencies:   mbed

Fork of Yusheng-final_project by Britney Dorval

main.cpp@10:5ef5fe8c7775, 2017-04-16 (annotated)

Committer:
britneyd
Date:
Sun Apr 16 19:52:52 2017 +0000
Revision:
10:5ef5fe8c7775
Parent:
9:a8fd0bd49279
Child:
11:dc410a980771
Final project;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
csharer 6:ae3e6aefe908 1 //Balance Bot V4
csharer 4:2512939c10f0 2 //Author: Carter Sharer
csharer 4:2512939c10f0 3 //Date: 10/13/2016
csharer 6:ae3e6aefe908 4 //ESE519 Lab 6 Part 3 Skeleton Code
csharer 3:2f76ffbc5cef 5
csharer 6:ae3e6aefe908 6 /******************************* README USAGE *******************************
csharer 9:a8fd0bd49279 7 * This robot must be powered on while it is laying down flat on a still table
csharer 6:ae3e6aefe908 8 * This allows the robot to calibrate the IMU (~5 seconds)
csharer 6:ae3e6aefe908 9 * The motors are DISABLED when the robot tilts more then +-45 degrees from
csharer 6:ae3e6aefe908 10 * vertical. To ENABLE the motors you must lift the robot to < +- 45 degres and
csharer 9:a8fd0bd49279 11 * press the joystick button.
csharer 6:ae3e6aefe908 12 * To reset the motor positions you must press the josystick button anytime.
csharer 6:ae3e6aefe908 13 ******************************************************************************/
csharer 6:ae3e6aefe908 14
csharer 6:ae3e6aefe908 15 //Balance Bot Begin
csharer 9:a8fd0bd49279 16 #include "mbed.h"
csharer 3:2f76ffbc5cef 17 #include "pin_assignments.h"
csharer 6:ae3e6aefe908 18 #include "balance_bot.h"
csharer 3:2f76ffbc5cef 19 #include "stepper_motors.h"
csharer 4:2512939c10f0 20 #include "MRF24J40.h"
csharer 4:2512939c10f0 21
britneyd 10:5ef5fe8c7775 22 #define WHEEL_BASE 100 //mm
britneyd 10:5ef5fe8c7775 23 #define NUM_STEPS 1600 //number of steps for 1 turn of wheel
britneyd 10:5ef5fe8c7775 24 #define WHEEL_RADIUS 51 //units in mm
britneyd 10:5ef5fe8c7775 25 #define PI 3.1415
britneyd 10:5ef5fe8c7775 26 #define maxAccel 5
britneyd 10:5ef5fe8c7775 27 //#define CIRC 2*PI*WHEEL_RADIUS
britneyd 10:5ef5fe8c7775 28
csharer 4:2512939c10f0 29 //For RF Communication
csharer 4:2512939c10f0 30 #define JSTICK_H 8
csharer 4:2512939c10f0 31 #define JSTICK_V 9
csharer 4:2512939c10f0 32 #define SPACE 10
csharer 4:2512939c10f0 33 #define KNOB1 11
csharer 4:2512939c10f0 34 #define KNOB2 12
csharer 4:2512939c10f0 35 #define KNOB3 13
csharer 4:2512939c10f0 36 #define KNOB4 14
csharer 4:2512939c10f0 37 #define BUTTON 16
csharer 4:2512939c10f0 38 #define JSTICK_OFFSET 100
csharer 4:2512939c10f0 39 #define TX_BUFFER_LEN 18
csharer 4:2512939c10f0 40 #define TX_ANGLE_OFFSET 100
csharer 4:2512939c10f0 41 //Knobs
csharer 4:2512939c10f0 42 #define POT1 p17
csharer 4:2512939c10f0 43 #define POT2 p18
csharer 4:2512939c10f0 44 #define POT3 p16
csharer 4:2512939c10f0 45 #define POT4 p15
csharer 4:2512939c10f0 46 //JoyStick
csharer 4:2512939c10f0 47 #define POTV p19
csharer 4:2512939c10f0 48 #define POTH p20
britneyd 10:5ef5fe8c7775 49 #define X 500
britneyd 10:5ef5fe8c7775 50 #define Y 0
britneyd 10:5ef5fe8c7775 51
csharer 3:2f76ffbc5cef 52
csharer 3:2f76ffbc5cef 53 //PID
csharer 6:ae3e6aefe908 54 #define MAX_THROTTLE 100
csharer 3:2f76ffbc5cef 55 #define MAX_TARGET_ANGLE 12
csharer 6:ae3e6aefe908 56 //PID Default control values from constant definitions
csharer 6:ae3e6aefe908 57 float Kp1;
csharer 6:ae3e6aefe908 58 float Kd1;
csharer 6:ae3e6aefe908 59 float Kp2;
csharer 6:ae3e6aefe908 60 float Kd2;
britneyd 10:5ef5fe8c7775 61 int i;
csharer 4:2512939c10f0 62
csharer 4:2512939c10f0 63 //Controller Values
csharer 4:2512939c10f0 64 uint8_t knob1, knob2, knob3, knob4;
csharer 4:2512939c10f0 65 int8_t jstick_h, jstick_v;
csharer 4:2512939c10f0 66
csharer 6:ae3e6aefe908 67 //Control Variables
csharer 6:ae3e6aefe908 68 float throttle = 0; //From joystick
csharer 6:ae3e6aefe908 69 float steering = 0; //From joystick
csharer 6:ae3e6aefe908 70 int robot_pos = 0; //Robots position
britneyd 10:5ef5fe8c7775 71 float motor1_old = 0;
csharer 3:2f76ffbc5cef 72
csharer 3:2f76ffbc5cef 73 Timer timer;
csharer 9:a8fd0bd49279 74 int timer_value;
csharer 9:a8fd0bd49279 75 int timer_old;
csharer 4:2512939c10f0 76 int dt;
csharer 3:2f76ffbc5cef 77
csharer 9:a8fd0bd49279 78 //Loop Counters
csharer 3:2f76ffbc5cef 79 uint8_t slow_loop_counter;
csharer 4:2512939c10f0 80 uint8_t medium_loop_counter;
csharer 3:2f76ffbc5cef 81 uint8_t loop_counter;
csharer 3:2f76ffbc5cef 82
csharer 3:2f76ffbc5cef 83 Serial pc(USBTX, USBRX);
csharer 3:2f76ffbc5cef 84
csharer 4:2512939c10f0 85 // LEDs
csharer 4:2512939c10f0 86 DigitalOut led1(LED1);
csharer 4:2512939c10f0 87 DigitalOut led2(LED2);
csharer 4:2512939c10f0 88 DigitalOut led3(LED3);
csharer 4:2512939c10f0 89 DigitalOut led4(LED4);
csharer 4:2512939c10f0 90
csharer 4:2512939c10f0 91 //Button
csharer 4:2512939c10f0 92 bool button;
csharer 6:ae3e6aefe908 93 #include "communication.h"
csharer 4:2512939c10f0 94
britneyd 10:5ef5fe8c7775 95 //Waypoint
britneyd 10:5ef5fe8c7775 96 //[(10m, 0th). (5m, 90th)]
britneyd 10:5ef5fe8c7775 97 //(10x, 0y), (x, y)]
britneyd 10:5ef5fe8c7775 98
csharer 3:2f76ffbc5cef 99 // ================================================================
csharer 3:2f76ffbc5cef 100 // === INITIAL SETUP ===
csharer 3:2f76ffbc5cef 101 // ================================================================
csharer 3:2f76ffbc5cef 102
csharer 3:2f76ffbc5cef 103 // ================================================================
csharer 3:2f76ffbc5cef 104 // === MAIN PROGRAM LOOP ===
csharer 3:2f76ffbc5cef 105 // ================================================================
csharer 3:2f76ffbc5cef 106 int main()
csharer 3:2f76ffbc5cef 107 {
csharer 9:a8fd0bd49279 108 //Used to toggle motors on and off
csharer 9:a8fd0bd49279 109 bool ROBOT_ON = false;
csharer 9:a8fd0bd49279 110
csharer 6:ae3e6aefe908 111 //Set the Channel. 0 is default, 15 is max
csharer 9:a8fd0bd49279 112 uint8_t channel = 9;
csharer 6:ae3e6aefe908 113 mrf.SetChannel(channel);
britneyd 10:5ef5fe8c7775 114
csharer 9:a8fd0bd49279 115 //Used for button press
csharer 9:a8fd0bd49279 116 int last_button_time = 0;
britneyd 10:5ef5fe8c7775 117 float wheel1_dist = 0;
britneyd 10:5ef5fe8c7775 118 float wheel2_dist = 0;
britneyd 10:5ef5fe8c7775 119 float robot_theta = 0;
britneyd 10:5ef5fe8c7775 120 float distance_traveled = 0;
britneyd 10:5ef5fe8c7775 121 float world_x = 0;
britneyd 10:5ef5fe8c7775 122 float world_y = 0;
britneyd 10:5ef5fe8c7775 123 float robot_pos_old = 0;
csharer 6:ae3e6aefe908 124
csharer 6:ae3e6aefe908 125 pc.baud(115200);
csharer 3:2f76ffbc5cef 126 pc.printf("Start\r\n");
csharer 3:2f76ffbc5cef 127 timer.start();
csharer 3:2f76ffbc5cef 128 //timer
csharer 4:2512939c10f0 129 timer_value = timer.read_us();
syundo0730 0:8d2c753a96e7 130
csharer 3:2f76ffbc5cef 131 //Init Stepper Motors
csharer 3:2f76ffbc5cef 132 //Attach Timer Interupts (Tiker)
csharer 3:2f76ffbc5cef 133 timer_M1.attach_us(&ISR1, ZERO_SPEED);
csharer 3:2f76ffbc5cef 134 timer_M2.attach_us(&ISR2, ZERO_SPEED);
csharer 3:2f76ffbc5cef 135 step_M1 = 1;
csharer 3:2f76ffbc5cef 136 dir_M1 = 1;
csharer 6:ae3e6aefe908 137 enable = DISABLE; //Disable Motors
csharer 4:2512939c10f0 138
csharer 6:ae3e6aefe908 139 //Enable Motors
csharer 6:ae3e6aefe908 140 enable = ENABLE;
csharer 6:ae3e6aefe908 141
csharer 3:2f76ffbc5cef 142 while(1) {
britneyd 10:5ef5fe8c7775 143 //Led 4 to indicate if robot is Running or Not
csharer 9:a8fd0bd49279 144 led4 = ROBOT_ON;
csharer 4:2512939c10f0 145
csharer 6:ae3e6aefe908 146 //Led 2 to indicate a button press
csharer 6:ae3e6aefe908 147 led2 = button;
csharer 6:ae3e6aefe908 148
csharer 6:ae3e6aefe908 149 //If button is pressed reset motor position
csharer 4:2512939c10f0 150 if(button) {
csharer 6:ae3e6aefe908 151 pos_M1 = 0; //Reset position of Motor 1
csharer 6:ae3e6aefe908 152 pos_M2 = 0; //Reset position of motor 2
britneyd 10:5ef5fe8c7775 153
britneyd 10:5ef5fe8c7775 154 //Read Button Press to Toggle Motors
csharer 9:a8fd0bd49279 155 if((timer.read_us() - last_button_time) > 250000) {
csharer 9:a8fd0bd49279 156 ROBOT_ON = ROBOT_ON^1;
csharer 9:a8fd0bd49279 157 pc.printf("BUTTON WAS PRESSED \r\n");
csharer 9:a8fd0bd49279 158 last_button_time = timer.read_us();
csharer 9:a8fd0bd49279 159 }
csharer 4:2512939c10f0 160 }
csharer 4:2512939c10f0 161
britneyd 10:5ef5fe8c7775 162 //Conver Motor Position from Steps to Distance
britneyd 10:5ef5fe8c7775 163 wheel1_dist = 2*PI*WHEEL_RADIUS* pos_M1/NUM_STEPS;
britneyd 10:5ef5fe8c7775 164 wheel2_dist = 2*PI*WHEEL_RADIUS* pos_M2/NUM_STEPS;
britneyd 10:5ef5fe8c7775 165 robot_pos = (wheel1_dist + wheel2_dist) / 2; ///total dist traveled
britneyd 10:5ef5fe8c7775 166 robot_theta = (wheel1_dist - wheel2_dist) /WHEEL_BASE; //angle
csharer 9:a8fd0bd49279 167
britneyd 10:5ef5fe8c7775 168 distance_traveled = robot_pos - robot_pos_old;
britneyd 10:5ef5fe8c7775 169
britneyd 10:5ef5fe8c7775 170
britneyd 10:5ef5fe8c7775 171 //Curr Robot X and Y
britneyd 10:5ef5fe8c7775 172 world_x += distance_traveled * sin(robot_theta); //import sin
britneyd 10:5ef5fe8c7775 173 world_y += distance_traveled * cos(robot_theta);
britneyd 10:5ef5fe8c7775 174
britneyd 10:5ef5fe8c7775 175 //Pose(x,y,th) = P(world_x, world_y, robot_th)
britneyd 10:5ef5fe8c7775 176
britneyd 10:5ef5fe8c7775 177 pc.printf("X axis: %d",world_x);
britneyd 10:5ef5fe8c7775 178 pc.printf("Y axis: %d \n",world_y);
britneyd 10:5ef5fe8c7775 179 pc.printf("robot_pos: %d \n",robot_pos);
csharer 9:a8fd0bd49279 180 //Timer
csharer 9:a8fd0bd49279 181 timer_value = timer.read_us();
csharer 4:2512939c10f0 182
csharer 9:a8fd0bd49279 183 //Joystick control
csharer 9:a8fd0bd49279 184 throttle = jstick_v;
csharer 9:a8fd0bd49279 185 steering = jstick_h;
csharer 4:2512939c10f0 186
csharer 9:a8fd0bd49279 187 /**** Update Values DO NOT MODIFY ********/
csharer 9:a8fd0bd49279 188 loop_counter++;
csharer 9:a8fd0bd49279 189 slow_loop_counter++;
csharer 9:a8fd0bd49279 190 medium_loop_counter++;
csharer 9:a8fd0bd49279 191 dt = (timer_value - timer_old);
csharer 9:a8fd0bd49279 192 timer_old = timer_value;
britneyd 10:5ef5fe8c7775 193 robot_pos_old = robot_pos;
csharer 9:a8fd0bd49279 194 /*****************************************/
csharer 4:2512939c10f0 195
csharer 9:a8fd0bd49279 196 //Running: Motor Control Enabled
csharer 9:a8fd0bd49279 197 if(ROBOT_ON) {
britneyd 10:5ef5fe8c7775 198 //TEMPWAYPOINTS
britneyd 10:5ef5fe8c7775 199 //Get Waypoint
britneyd 10:5ef5fe8c7775 200 //i = 0;
britneyd 10:5ef5fe8c7775 201 //X = WAYPOINS[i][0]
britneyd 10:5ef5fe8c7775 202 //Y = WAYPOINS[i][1]
britneyd 10:5ef5fe8c7775 203 //calc target theta
britneyd 10:5ef5fe8c7775 204 float target_theta = 0;
britneyd 10:5ef5fe8c7775 205 float target_distance = 5000;
britneyd 10:5ef5fe8c7775 206 //if(robot is at waypoint)
britneyd 10:5ef5fe8c7775 207 //get new waypoint
britneyd 10:5ef5fe8c7775 208 //i++
britneyd 10:5ef5fe8c7775 209 //else {
britneyd 10:5ef5fe8c7775 210 //}
britneyd 10:5ef5fe8c7775 211
britneyd 10:5ef5fe8c7775 212
britneyd 10:5ef5fe8c7775 213
csharer 9:a8fd0bd49279 214 //Enable Motor
csharer 9:a8fd0bd49279 215 enable = ENABLE;
csharer 3:2f76ffbc5cef 216
britneyd 10:5ef5fe8c7775 217 //User Control ONLY
britneyd 10:5ef5fe8c7775 218 float theta_error = target_theta - robot_theta;
britneyd 10:5ef5fe8c7775 219
britneyd 10:5ef5fe8c7775 220 if(theta_error > 1 && theta_error < -1) {
britneyd 10:5ef5fe8c7775 221 //Angular Controller
britneyd 10:5ef5fe8c7775 222 float turn_speed = theta_error*0.5;
britneyd 10:5ef5fe8c7775 223 motor1 = turn_speed;
britneyd 10:5ef5fe8c7775 224 motor2 = -turn_speed;
britneyd 10:5ef5fe8c7775 225 } else {
britneyd 10:5ef5fe8c7775 226 //Dist Controller
britneyd 10:5ef5fe8c7775 227 float dist_error = target_distance - robot_pos;
britneyd 10:5ef5fe8c7775 228 float speed = dist_error*0.5;
britneyd 10:5ef5fe8c7775 229 motor1 = speed;
britneyd 10:5ef5fe8c7775 230 motor2 = speed;
britneyd 10:5ef5fe8c7775 231 }
britneyd 10:5ef5fe8c7775 232
britneyd 10:5ef5fe8c7775 233
britneyd 10:5ef5fe8c7775 234 //Acceleratoin Cap
britneyd 10:5ef5fe8c7775 235 float motor1Acel = motor1 - motor1_old;
britneyd 10:5ef5fe8c7775 236
britneyd 10:5ef5fe8c7775 237 if(motor1Acel > maxAccel) { //macAcel = 5
britneyd 10:5ef5fe8c7775 238 motor1 = maxAccel;
britneyd 10:5ef5fe8c7775 239 }
csharer 9:a8fd0bd49279 240
csharer 9:a8fd0bd49279 241 //Cap the max and min values [-100, 100]
csharer 9:a8fd0bd49279 242 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
csharer 9:a8fd0bd49279 243 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
britneyd 10:5ef5fe8c7775 244 motor1_old = motor1;
csharer 3:2f76ffbc5cef 245
csharer 9:a8fd0bd49279 246 //Set Motor Speed here
csharer 9:a8fd0bd49279 247 setMotor1Speed(motor1);
csharer 9:a8fd0bd49279 248 setMotor2Speed(motor2);
csharer 9:a8fd0bd49279 249
britneyd 10:5ef5fe8c7775 250 }
csharer 9:a8fd0bd49279 251 //Robot is off so disable the motors
csharer 9:a8fd0bd49279 252 else {
csharer 9:a8fd0bd49279 253 enable = DISABLE;
csharer 9:a8fd0bd49279 254 }
csharer 9:a8fd0bd49279 255
csharer 9:a8fd0bd49279 256 /* Here are some loops that trigger at different intervals, this
csharer 9:a8fd0bd49279 257 * will allow you to do things at a slower rate, thus saving speed
csharer 9:a8fd0bd49279 258 * it is important to keep this fast so we dont miss IMU readings */
csharer 4:2512939c10f0 259
csharer 9:a8fd0bd49279 260 //Fast Loop: Good for printing to serial monitor
csharer 9:a8fd0bd49279 261 if(loop_counter >= 5) {
csharer 9:a8fd0bd49279 262 loop_counter = 0;
csharer 9:a8fd0bd49279 263 pc.printf("ROBOT_ON:%d pos_M1:%d pos_M2:%d robot_pos%d \r\n", ROBOT_ON, pos_M1, pos_M2, robot_pos);
csharer 9:a8fd0bd49279 264 }
csharer 6:ae3e6aefe908 265
csharer 9:a8fd0bd49279 266 //Meduim Loop: Good for sending and receiving
csharer 9:a8fd0bd49279 267 if (medium_loop_counter >= 10) {
csharer 9:a8fd0bd49279 268 medium_loop_counter = 0; // Read status
csharer 9:a8fd0bd49279 269
csharer 9:a8fd0bd49279 270 //Recieve Data
csharer 9:a8fd0bd49279 271 rxLen = rf_receive(rxBuffer, 128);
csharer 9:a8fd0bd49279 272 if(rxLen > 0) {
csharer 9:a8fd0bd49279 273 led1 = led1^1;
csharer 9:a8fd0bd49279 274 //Process data with our protocal
csharer 9:a8fd0bd49279 275 communication_protocal(rxLen);
csharer 4:2512939c10f0 276 }
csharer 3:2f76ffbc5cef 277
csharer 9:a8fd0bd49279 278 } // End of medium loop
csharer 6:ae3e6aefe908 279
csharer 9:a8fd0bd49279 280 //Slow Loop: Good for sending if speed is not an issue
csharer 9:a8fd0bd49279 281 if(slow_loop_counter >= 99) {
csharer 9:a8fd0bd49279 282 slow_loop_counter = 0;
csharer 3:2f76ffbc5cef 283
csharer 9:a8fd0bd49279 284 /* Send Data To Controller goes here *
csharer 9:a8fd0bd49279 285 * */
csharer 9:a8fd0bd49279 286 } //End of Slow Loop
csharer 4:2512939c10f0 287
csharer 6:ae3e6aefe908 288
csharer 3:2f76ffbc5cef 289
csharer 3:2f76ffbc5cef 290 } //end main loop
csharer 3:2f76ffbc5cef 291 } //End Main()