Britney Dorval
Final project with problem
Fork of
Yusheng-final_project
by 
Carter Sharer
Diff: main.cpp
- Revision:
- 9:a8fd0bd49279
- Parent:
- 6:ae3e6aefe908
- Child:
- 10:5ef5fe8c7775
--- a/main.cpp Fri Nov 11 19:20:41 2016 +0000
+++ b/main.cpp Thu Mar 30 22:13:44 2017 +0000
@@ -4,27 +4,21 @@
//ESE519 Lab 6 Part 3 Skeleton Code
/******************************* README USAGE *******************************
-* This robot must be powered on while it is laying down flat on a still table
+* This robot must be powered on while it is laying down flat on a still table
* This allows the robot to calibrate the IMU (~5 seconds)
* The motors are DISABLED when the robot tilts more then +-45 degrees from
* vertical. To ENABLE the motors you must lift the robot to < +- 45 degres and
-* press the joystick button.
+* press the joystick button.
* To reset the motor positions you must press the josystick button anytime.
******************************************************************************/
//Balance Bot Begin
+#include "mbed.h"
#include "pin_assignments.h"
-#include "I2Cdev.h"
-#include "JJ_MPU6050_DMP_6Axis.h"
#include "balance_bot.h"
-#include "balance_bot_IMU.h"
#include "stepper_motors.h"
#include "MRF24J40.h"
-//Angle Offset is used to set the natural balance point of your robot.
-//You should adjust this offset so that your robots balance point is near 0
-#define ANGLE_OFFSET 107
-
//For RF Communication
#define JSTICK_H 8
#define JSTICK_V 9
@@ -33,7 +27,6 @@
#define KNOB2 12
#define KNOB3 13
#define KNOB4 14
-#define ANGLE 15
#define BUTTON 16
#define JSTICK_OFFSET 100
#define TX_BUFFER_LEN 18
@@ -61,19 +54,16 @@
int8_t jstick_h, jstick_v;
//Control Variables
-float target_angle;
float throttle = 0; //From joystick
float steering = 0; //From joystick
-float max_target_angle = MAX_TARGET_ANGLE;
int robot_pos = 0; //Robots position
-bool fallen = true;
Timer timer;
-int timer_value;
-int timer_old;
+int timer_value;
+int timer_old;
int dt;
-//Loop Counters
+//Loop Counters
uint8_t slow_loop_counter;
uint8_t medium_loop_counter;
uint8_t loop_counter;
@@ -93,58 +83,24 @@
// ================================================================
// === INITIAL SETUP ===
// ================================================================
-void init_imu()
-{
- pc.printf("\r\r\n\n Start \r\n");
-
- // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
- mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
- mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
- mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
- mpu.setDLPFMode(MPU6050_DLPF_BW_10); //10,20,42,98,188 // Default factor for BROBOT:10
- mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
- mpu.setSleepEnabled(false);
- wait_ms(500);
-
- // load and configure the DMP
- devStatus = mpu.dmpInitialize();
- if(devStatus == 0) {
- mpu.setDMPEnabled(true);
- mpuIntStatus = mpu.getIntStatus();
- dmpReady = true;
- } else {
- // 1 = initial memory load failed
- // 2 = DMP configuration updates failed
- pc.printf("DMP INIT error \r\n");
- }
-
- //Gyro Calibration
- wait_ms(500);
- pc.printf("Gyro calibration!! Dont move the robot in 10 seconds... \r\n");
- wait_ms(500);
-
- // verify connection
- pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");
-
- //Adjust Sensor Fusion Gain
- dmpSetSensorFusionAccelGain(0x20);
-
- wait_ms(200);
- mpu.resetFIFO();
-}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
int main()
{
+ //Used to toggle motors on and off
+ bool ROBOT_ON = false;
+
//Set the Channel. 0 is default, 15 is max
- uint8_t channel = 2;
+ uint8_t channel = 9;
mrf.SetChannel(channel);
+
+ //Used for button press
+ int last_button_time = 0;
pc.baud(115200);
pc.printf("Start\r\n");
- init_imu();
timer.start();
//timer
timer_value = timer.read_us();
@@ -157,18 +113,12 @@
dir_M1 = 1;
enable = DISABLE; //Disable Motors
- //Attach Interupt for IMU
- checkpin.rise(&dmpDataReady);
-
- //Used to set angle upon startup, filter
- bool FILTER_DISABLE = true;
-
//Enable Motors
enable = ENABLE;
while(1) {
- //Led 4 to indicate if robot it STANDING or FALLEN
- led4 = !fallen;
+ //Led 4 to indicate if robot it Running or Not
+ led4 = ROBOT_ON;
//Led 2 to indicate a button press
led2 = button;
@@ -177,143 +127,86 @@
if(button) {
pos_M1 = 0; //Reset position of Motor 1
pos_M2 = 0; //Reset position of motor 2
- fallen = false; //Reset fallen flag
+
+ if((timer.read_us() - last_button_time) > 250000) {
+ ROBOT_ON = ROBOT_ON^1;
+ pc.printf("BUTTON WAS PRESSED \r\n");
+ last_button_time = timer.read_us();
+ }
}
- //This is the main while loop, all your code goes here
- while(!mpuInterrupt) {
- //Timer
- timer_value = timer.read_us();
+
+ //Timer
+ timer_value = timer.read_us();
- //Set gainz with knobs
- Kp1 = knob1;
- Kd1 = knob2;
- Kp2 = knob3;
- Kd2 = knob4;
+ //Joystick control
+ throttle = jstick_v;
+ steering = jstick_h;
- //Joystick control
- throttle = jstick_v;
- steering = jstick_h;
+ /**** Update Values DO NOT MODIFY ********/
+ loop_counter++;
+ slow_loop_counter++;
+ medium_loop_counter++;
+ dt = (timer_value - timer_old);
+ timer_old = timer_value;
+ /*****************************************/
- /**** Update Values DO NOT MODIFY ********/
- loop_counter++;
- slow_loop_counter++;
- medium_loop_counter++;
- dt = (timer_value - timer_old);
- timer_old = timer_value;
- angle_old = angle;
- /*****************************************/
+ //Running: Motor Control Enabled
+ if(ROBOT_ON) {
+ //Enable Motor
+ enable = ENABLE;
- //STANDING: Motor Control Enabled
- if(((angle < 45) && (angle > -45)) && (fallen == false)) {
+ //Calculate motor inputs
+ motor1 = int16_t(throttle/2 + steering/8);
+ motor2 = int16_t(throttle/2 - steering/8);
+
+ //Cap the max and min values [-100, 100]
+ motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
+ motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
- //Enable Motor
- enable = ENABLE;
-
- /* This is where you want to impliment your controlers
- * Start off with a simple P controller.
- *
- * float error = angle - 0; //should be balanced at 0
- * motor1 = error * Kp;
- * motor2 = error * Kp; */
-
- //Calculate motor inputs
- motor1 = int16_t(throttle/2 + steering/8);
- motor2 = int16_t(throttle/2 - steering/8);
-
- //Cap the max and min values [-100, 100]
- motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
- motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
-
- //Set Motor Speed here
- setMotor1Speed(motor1);
- setMotor2Speed(motor2);
+ //Set Motor Speed here
+ setMotor1Speed(motor1);
+ setMotor2Speed(motor2);
+
+ }
+ //Robot is off so disable the motors
+ else {
+ enable = DISABLE;
+ }
+
+ /* Here are some loops that trigger at different intervals, this
+ * will allow you to do things at a slower rate, thus saving speed
+ * it is important to keep this fast so we dont miss IMU readings */
- } else { //FALLEN: Motor Control Disabled
- //Disable Motors
- enable = DISABLE;
+ //Fast Loop: Good for printing to serial monitor
+ if(loop_counter >= 5) {
+ loop_counter = 0;
+ pc.printf("ROBOT_ON:%d pos_M1:%d pos_M2:%d robot_pos%d \r\n", ROBOT_ON, pos_M1, pos_M2, robot_pos);
+ }
- //Set fallen flag
- fallen = true;
- }
-
- /* Here are some loops that trigger at different intervals, this
- * will allow you to do things at a slower rate, thus saving speed
- * it is important to keep this fast so we dont miss IMU readings */
-
- //Fast Loop: Good for printing to serial monitor
- if(loop_counter >= 5) {
- loop_counter = 0;
- pc.printf("angle:%d Kp1:%0.3f Kd1:%0.2f Kp2:%0.2f Kd2:%0.3f pos_M1:%d pos_M2:%d \r\n", (int)angle, Kp1, Kd1, Kp2, Kd2, pos_M1, pos_M2, robot_pos);
+ //Meduim Loop: Good for sending and receiving
+ if (medium_loop_counter >= 10) {
+ medium_loop_counter = 0; // Read status
+
+ //Recieve Data
+ rxLen = rf_receive(rxBuffer, 128);
+ if(rxLen > 0) {
+ led1 = led1^1;
+ //Process data with our protocal
+ communication_protocal(rxLen);
}
- //Meduim Loop: Good for sending and receiving
- if (medium_loop_counter >= 10) {
- medium_loop_counter = 0; // Read status
-
- //Recieve Data
- rxLen = rf_receive(rxBuffer, 128);
- if(rxLen > 0) {
- led1 = led1^1;
- //Process data with our protocal
- communication_protocal(rxLen);
- }
+ } // End of medium loop
- } // End of medium loop
-
- //Slow Loop: Good for sending if speed is not an issue
- if(slow_loop_counter >= 99) {
- slow_loop_counter = 0;
+ //Slow Loop: Good for sending if speed is not an issue
+ if(slow_loop_counter >= 99) {
+ slow_loop_counter = 0;
- /* Send Data To Controller goes here *
- * */
- } //End of Slow Loop
-
- //Reattach interupt
- checkpin.rise(&dmpDataReady);
- } //END WHILE
+ /* Send Data To Controller goes here *
+ * */
+ } //End of Slow Loop
- /********************* All IMU Handling DO NOT MODIFY *****************/
- //Disable IRQ
- checkpin.rise(NULL);
-
- //reset interrupt flag and get INT_STATUS byte
- mpuInterrupt = false;
- mpuIntStatus = mpu.getIntStatus();
-
- //get current FIFO count
- fifoCount = mpu.getFIFOCount();
-
- // check for overflow (this should never happen unless our code is too inefficient)
- if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
- // reset so we can continue cleanly
- mpu.resetFIFO();
- pc.printf("FIFO overflow!");
-
- //otherwise, check for DMP data ready interrupt (this should happen frequently)
- } else if (mpuIntStatus & 0x02) {
- //wait for correct available data length, should be a VERY short wait
- while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
-
- //read a packet from FIFO
- mpu.getFIFOBytes(fifoBuffer, packetSize);
-
- //track FIFO count here in case there is > 1 packet available
- //(this lets us immediately read more without waiting for an interrupt)
- fifoCount -= packetSize;
-
- //Read new angle from IMU
- new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
- dAngle = new_angle - angle;
-
- //Filter out angle readings larger then MAX_ANGLE_DELTA
- if( ((dAngle < 15) && (dAngle > -15)) || FILTER_DISABLE) {
- angle = new_angle;
- FILTER_DISABLE = false; //turn of filter disabler
- }
- }
- /********************* All IMU Handling DO NOT MODIFY *****************/
} //end main loop
} //End Main()
\ No newline at end of file