Britney Dorval / Mbed 2 deprecated Yusheng-final_project_robot

Code for robot

Dependencies:   mbed

Fork of Yusheng-final_project by Britney Dorval

main.cpp@9:a8fd0bd49279, 2017-03-30 (annotated)

Committer:
csharer
Date:
Thu Mar 30 22:13:44 2017 +0000
Revision:
9:a8fd0bd49279
Parent:
6:ae3e6aefe908
Child:
10:5ef5fe8c7775
Final Project Starter Code

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
csharer 4:2512939c10f0 22 //For RF Communication
csharer 4:2512939c10f0 23 #define JSTICK_H 8
csharer 4:2512939c10f0 24 #define JSTICK_V 9
csharer 4:2512939c10f0 25 #define SPACE 10
csharer 4:2512939c10f0 26 #define KNOB1 11
csharer 4:2512939c10f0 27 #define KNOB2 12
csharer 4:2512939c10f0 28 #define KNOB3 13
csharer 4:2512939c10f0 29 #define KNOB4 14
csharer 4:2512939c10f0 30 #define BUTTON 16
csharer 4:2512939c10f0 31 #define JSTICK_OFFSET 100
csharer 4:2512939c10f0 32 #define TX_BUFFER_LEN 18
csharer 4:2512939c10f0 33 #define TX_ANGLE_OFFSET 100
csharer 4:2512939c10f0 34 //Knobs
csharer 4:2512939c10f0 35 #define POT1 p17
csharer 4:2512939c10f0 36 #define POT2 p18
csharer 4:2512939c10f0 37 #define POT3 p16
csharer 4:2512939c10f0 38 #define POT4 p15
csharer 4:2512939c10f0 39 //JoyStick
csharer 4:2512939c10f0 40 #define POTV p19
csharer 4:2512939c10f0 41 #define POTH p20
csharer 3:2f76ffbc5cef 42
csharer 3:2f76ffbc5cef 43 //PID
csharer 6:ae3e6aefe908 44 #define MAX_THROTTLE 100
csharer 3:2f76ffbc5cef 45 #define MAX_TARGET_ANGLE 12
csharer 6:ae3e6aefe908 46 //PID Default control values from constant definitions
csharer 6:ae3e6aefe908 47 float Kp1;
csharer 6:ae3e6aefe908 48 float Kd1;
csharer 6:ae3e6aefe908 49 float Kp2;
csharer 6:ae3e6aefe908 50 float Kd2;
csharer 4:2512939c10f0 51
csharer 4:2512939c10f0 52 //Controller Values
csharer 4:2512939c10f0 53 uint8_t knob1, knob2, knob3, knob4;
csharer 4:2512939c10f0 54 int8_t jstick_h, jstick_v;
csharer 4:2512939c10f0 55
csharer 6:ae3e6aefe908 56 //Control Variables
csharer 6:ae3e6aefe908 57 float throttle = 0; //From joystick
csharer 6:ae3e6aefe908 58 float steering = 0; //From joystick
csharer 6:ae3e6aefe908 59 int robot_pos = 0; //Robots position
csharer 3:2f76ffbc5cef 60
csharer 3:2f76ffbc5cef 61 Timer timer;
csharer 9:a8fd0bd49279 62 int timer_value;
csharer 9:a8fd0bd49279 63 int timer_old;
csharer 4:2512939c10f0 64 int dt;
csharer 3:2f76ffbc5cef 65
csharer 9:a8fd0bd49279 66 //Loop Counters
csharer 3:2f76ffbc5cef 67 uint8_t slow_loop_counter;
csharer 4:2512939c10f0 68 uint8_t medium_loop_counter;
csharer 3:2f76ffbc5cef 69 uint8_t loop_counter;
csharer 3:2f76ffbc5cef 70
csharer 3:2f76ffbc5cef 71 Serial pc(USBTX, USBRX);
csharer 3:2f76ffbc5cef 72
csharer 4:2512939c10f0 73 // LEDs
csharer 4:2512939c10f0 74 DigitalOut led1(LED1);
csharer 4:2512939c10f0 75 DigitalOut led2(LED2);
csharer 4:2512939c10f0 76 DigitalOut led3(LED3);
csharer 4:2512939c10f0 77 DigitalOut led4(LED4);
csharer 4:2512939c10f0 78
csharer 4:2512939c10f0 79 //Button
csharer 4:2512939c10f0 80 bool button;
csharer 6:ae3e6aefe908 81 #include "communication.h"
csharer 4:2512939c10f0 82
csharer 3:2f76ffbc5cef 83 // ================================================================
csharer 3:2f76ffbc5cef 84 // === INITIAL SETUP ===
csharer 3:2f76ffbc5cef 85 // ================================================================
csharer 3:2f76ffbc5cef 86
csharer 3:2f76ffbc5cef 87 // ================================================================
csharer 3:2f76ffbc5cef 88 // === MAIN PROGRAM LOOP ===
csharer 3:2f76ffbc5cef 89 // ================================================================
csharer 3:2f76ffbc5cef 90 int main()
csharer 3:2f76ffbc5cef 91 {
csharer 9:a8fd0bd49279 92 //Used to toggle motors on and off
csharer 9:a8fd0bd49279 93 bool ROBOT_ON = false;
csharer 9:a8fd0bd49279 94
csharer 6:ae3e6aefe908 95 //Set the Channel. 0 is default, 15 is max
csharer 9:a8fd0bd49279 96 uint8_t channel = 9;
csharer 6:ae3e6aefe908 97 mrf.SetChannel(channel);
csharer 9:a8fd0bd49279 98
csharer 9:a8fd0bd49279 99 //Used for button press
csharer 9:a8fd0bd49279 100 int last_button_time = 0;
csharer 6:ae3e6aefe908 101
csharer 6:ae3e6aefe908 102 pc.baud(115200);
csharer 3:2f76ffbc5cef 103 pc.printf("Start\r\n");
csharer 3:2f76ffbc5cef 104 timer.start();
csharer 3:2f76ffbc5cef 105 //timer
csharer 4:2512939c10f0 106 timer_value = timer.read_us();
syundo0730 0:8d2c753a96e7 107
csharer 3:2f76ffbc5cef 108 //Init Stepper Motors
csharer 3:2f76ffbc5cef 109 //Attach Timer Interupts (Tiker)
csharer 3:2f76ffbc5cef 110 timer_M1.attach_us(&ISR1, ZERO_SPEED);
csharer 3:2f76ffbc5cef 111 timer_M2.attach_us(&ISR2, ZERO_SPEED);
csharer 3:2f76ffbc5cef 112 step_M1 = 1;
csharer 3:2f76ffbc5cef 113 dir_M1 = 1;
csharer 6:ae3e6aefe908 114 enable = DISABLE; //Disable Motors
csharer 4:2512939c10f0 115
csharer 6:ae3e6aefe908 116 //Enable Motors
csharer 6:ae3e6aefe908 117 enable = ENABLE;
csharer 6:ae3e6aefe908 118
csharer 3:2f76ffbc5cef 119 while(1) {
csharer 9:a8fd0bd49279 120 //Led 4 to indicate if robot it Running or Not
csharer 9:a8fd0bd49279 121 led4 = ROBOT_ON;
csharer 4:2512939c10f0 122
csharer 6:ae3e6aefe908 123 //Led 2 to indicate a button press
csharer 6:ae3e6aefe908 124 led2 = button;
csharer 6:ae3e6aefe908 125
csharer 6:ae3e6aefe908 126 //If button is pressed reset motor position
csharer 4:2512939c10f0 127 if(button) {
csharer 6:ae3e6aefe908 128 pos_M1 = 0; //Reset position of Motor 1
csharer 6:ae3e6aefe908 129 pos_M2 = 0; //Reset position of motor 2
csharer 9:a8fd0bd49279 130
csharer 9:a8fd0bd49279 131 if((timer.read_us() - last_button_time) > 250000) {
csharer 9:a8fd0bd49279 132 ROBOT_ON = ROBOT_ON^1;
csharer 9:a8fd0bd49279 133 pc.printf("BUTTON WAS PRESSED \r\n");
csharer 9:a8fd0bd49279 134 last_button_time = timer.read_us();
csharer 9:a8fd0bd49279 135 }
csharer 4:2512939c10f0 136 }
csharer 4:2512939c10f0 137
csharer 9:a8fd0bd49279 138
csharer 9:a8fd0bd49279 139 //Timer
csharer 9:a8fd0bd49279 140 timer_value = timer.read_us();
csharer 4:2512939c10f0 141
csharer 9:a8fd0bd49279 142 //Joystick control
csharer 9:a8fd0bd49279 143 throttle = jstick_v;
csharer 9:a8fd0bd49279 144 steering = jstick_h;
csharer 4:2512939c10f0 145
csharer 9:a8fd0bd49279 146 /**** Update Values DO NOT MODIFY ********/
csharer 9:a8fd0bd49279 147 loop_counter++;
csharer 9:a8fd0bd49279 148 slow_loop_counter++;
csharer 9:a8fd0bd49279 149 medium_loop_counter++;
csharer 9:a8fd0bd49279 150 dt = (timer_value - timer_old);
csharer 9:a8fd0bd49279 151 timer_old = timer_value;
csharer 9:a8fd0bd49279 152 /*****************************************/
csharer 4:2512939c10f0 153
csharer 9:a8fd0bd49279 154 //Running: Motor Control Enabled
csharer 9:a8fd0bd49279 155 if(ROBOT_ON) {
csharer 9:a8fd0bd49279 156 //Enable Motor
csharer 9:a8fd0bd49279 157 enable = ENABLE;
csharer 3:2f76ffbc5cef 158
csharer 9:a8fd0bd49279 159 //Calculate motor inputs
csharer 9:a8fd0bd49279 160 motor1 = int16_t(throttle/2 + steering/8);
csharer 9:a8fd0bd49279 161 motor2 = int16_t(throttle/2 - steering/8);
csharer 9:a8fd0bd49279 162
csharer 9:a8fd0bd49279 163 //Cap the max and min values [-100, 100]
csharer 9:a8fd0bd49279 164 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
csharer 9:a8fd0bd49279 165 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 166
csharer 9:a8fd0bd49279 167 //Set Motor Speed here
csharer 9:a8fd0bd49279 168 setMotor1Speed(motor1);
csharer 9:a8fd0bd49279 169 setMotor2Speed(motor2);
csharer 9:a8fd0bd49279 170
csharer 9:a8fd0bd49279 171 }
csharer 9:a8fd0bd49279 172 //Robot is off so disable the motors
csharer 9:a8fd0bd49279 173 else {
csharer 9:a8fd0bd49279 174 enable = DISABLE;
csharer 9:a8fd0bd49279 175 }
csharer 9:a8fd0bd49279 176
csharer 9:a8fd0bd49279 177 /* Here are some loops that trigger at different intervals, this
csharer 9:a8fd0bd49279 178 * will allow you to do things at a slower rate, thus saving speed
csharer 9:a8fd0bd49279 179 * it is important to keep this fast so we dont miss IMU readings */
csharer 4:2512939c10f0 180
csharer 9:a8fd0bd49279 181 //Fast Loop: Good for printing to serial monitor
csharer 9:a8fd0bd49279 182 if(loop_counter >= 5) {
csharer 9:a8fd0bd49279 183 loop_counter = 0;
csharer 9:a8fd0bd49279 184 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 185 }
csharer 6:ae3e6aefe908 186
csharer 9:a8fd0bd49279 187 //Meduim Loop: Good for sending and receiving
csharer 9:a8fd0bd49279 188 if (medium_loop_counter >= 10) {
csharer 9:a8fd0bd49279 189 medium_loop_counter = 0; // Read status
csharer 9:a8fd0bd49279 190
csharer 9:a8fd0bd49279 191 //Recieve Data
csharer 9:a8fd0bd49279 192 rxLen = rf_receive(rxBuffer, 128);
csharer 9:a8fd0bd49279 193 if(rxLen > 0) {
csharer 9:a8fd0bd49279 194 led1 = led1^1;
csharer 9:a8fd0bd49279 195 //Process data with our protocal
csharer 9:a8fd0bd49279 196 communication_protocal(rxLen);
csharer 4:2512939c10f0 197 }
csharer 3:2f76ffbc5cef 198
csharer 9:a8fd0bd49279 199 } // End of medium loop
csharer 6:ae3e6aefe908 200
csharer 9:a8fd0bd49279 201 //Slow Loop: Good for sending if speed is not an issue
csharer 9:a8fd0bd49279 202 if(slow_loop_counter >= 99) {
csharer 9:a8fd0bd49279 203 slow_loop_counter = 0;
csharer 3:2f76ffbc5cef 204
csharer 9:a8fd0bd49279 205 /* Send Data To Controller goes here *
csharer 9:a8fd0bd49279 206 * */
csharer 9:a8fd0bd49279 207 } //End of Slow Loop
csharer 4:2512939c10f0 208
csharer 6:ae3e6aefe908 209
csharer 3:2f76ffbc5cef 210
csharer 3:2f76ffbc5cef 211 } //end main loop
csharer 3:2f76ffbc5cef 212 } //End Main()