Carter Sharer
BroBot Code for ESE350 Lab6 part 3 (Skeleton)
Dependencies: MPU6050_V3 mbed-rtos mbed
Fork of
BroBot_RTOS_ESE350
by 
Carter Sharer
Diff: main.cpp
- Revision:
- 9:fb671fa0c0be
- Parent:
- 8:8389c0a9339e
- Child:
- 10:48f640a54401
--- a/main.cpp Sat Dec 17 23:58:58 2016 +0000
+++ b/main.cpp Mon Jan 09 17:50:35 2017 +0000
@@ -1,10 +1,22 @@
-//BroBot V3
+//BroBot V3 this is the most up to date, velocity is now working
//Author: Carter Sharer
//Date: 10/13/2016
//Added communication protocol v1 (no type selection)
-//Jstick_v: 0 Knob1 39.898 Knob2 44.381 Knob3 10.55 Knob4 18.67 Button: 0
+//Knob1 39.898 Knob2 44.381 Knob3 10.55 Knob4 18.67
+//Knob1 13.418 Knob2 28.848 Knob3 9.45 Knob4 42.29 Good
+//Knob1 15.373 Knob2 28.261 Knob3 10.42 Knob4 40.97 Smoother
+
+/******************************* README USAGE *******************************
+* 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.
+* To reset the motor positions you must press the josystick button anytime.
+******************************************************************************/
//BroBot Begin
+#include "cmsis_os.h"
#include "pin_assignments.h"
#include "I2Cdev.h"
#include "JJ_MPU6050_DMP_6Axis.h"
@@ -13,7 +25,7 @@
#include "stepper_motors.h"
#include "MRF24J40.h"
-//Angle Offset is used to set the natural balance point of your robot.
+//Angle Offset is used to set the natural balance point of your robot.
//You should adjust this offset so that your robots balance points is near 0
#define ANGLE_OFFSET 107
@@ -75,6 +87,7 @@
int16_t actual_robot_speed_old;
float estimated_speed_filtered; // Estimated robot speed
int robot_pos = 0;
+float velocity_error;
Timer timer;
int timer_value; //maybe make this a long
@@ -97,6 +110,68 @@
bool button;
#include "communication.h"
+//Used to set angle upon startup, filter
+bool FILTER_DISABLE = true;
+
+// ================================================================
+// === Threads ===
+// ================================================================
+void led2_thread(void const *args)
+{
+ while (true) {
+ //if(mpuInterrupt) {
+ led3 = !led3;
+ /********************* 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
+ } else {
+ pc.printf("\t\t\t filtered angle \r\n");
+ }
+ }
+ /********************* All IMU Handling DO NOT MODIFY *****************/
+ //}
+
+
+ osDelay(1);
+ //pc.printf(" thread2 ");
+ }
+}
+osThreadDef(led2_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
+
// ================================================================
// === INITIAL SETUP ===
// ================================================================
@@ -161,14 +236,14 @@
float change_in_pos;
float robot_pos_old1, robot_pos_old2;
float velocity;
+bool fallen = true;
-bool fallen = true;
int main()
{
//Set the Channel. 0 is default, 15 is max
uint8_t channel = 2;
mrf.SetChannel(channel);
-
+
pc.baud(115200);
pc.printf("Start\r\n");
init_imu();
@@ -188,21 +263,24 @@
checkpin.rise(&dmpDataReady);
//Used to set angle upon startup, filter
- bool FILTER_DISABLE = true;
+ //bool FILTER_DISABLE = true;
- //Enable Motors
+ //Enable Motors
enable = ENABLE;
+ //Create Threads
+ osThreadCreate(osThread(led2_thread), NULL);
+
while(1) {
//led1 = led1^1;
led4 = !fallen;
led2 = button;
-
+
if(jstick_v > 80)
led3 = 1;
else
led3 = 0;
-
+
if(button) {
pos_M1 = 0;
pos_M2 = 0;
@@ -210,9 +288,15 @@
fallen = false;
}
- while(!mpuInterrupt) {
+ // while(!mpuInterrupt) {
timer_value = timer.read_us();
+ //Preset Knob Values for Green Balance Bot
+ knob1 = 42.157;
+ knob2 = 41.135;
+ knob3 = 8.82;
+ knob4 = 20.68;
+
//Set Gainz with knobs
Kp1 = ((float)knob1) / 1000.0;
Kd1 = ((float)knob2) / 1.0;
@@ -231,23 +315,28 @@
timer_old = timer_value;
angle_old = angle;
- // STANDING: Motor Control Enabled
+ // STANDING: Motor Control Enabled
if(((angle < 45) && (angle > -45)) && (fallen == false)) {
//CS Pd Target Angle Contoller Goes Here
-
+
//Robot Position
robot_pos = (pos_M1 + pos_M2) / 2;
target_pos += throttle/2;
-
- velocity = motor1 + motor2 / 2;
-
- //CS ***************** Position error *************************
+
+ //Robot Current Velocity
+ velocity = motor1 + motor2 / 2;
+
+ //CS ***************** Velocity Controller *********************
+ //float target_velocity = 0;
+ //velocity_error = target_velocity - velocity;
+ //target_angle = -velocity_error * Kp2 * 100;
+ //CS ***************** Position Controller *********************
pos_error = robot_pos - target_pos; //robot_pos - target_pos;
-
- //KP Term
+
+ //KP Term
kp_pos_term = -Kp2 * pos_error;
-
+
//KD Term
change_in_target_pos = target_pos - target_pos_old;
change_in_pos = robot_pos - robot_pos_old2;
@@ -255,12 +344,12 @@
kd_pos_term = ((Kd2 * change_in_target_pos) + (Kd2*velocity));
target_angle = kp_pos_term + kd_pos_term;
target_angle = CAP(target_angle, MAX_TARGET_ANGLE);
-
+
//Update values
target_pos_old = target_pos;
robot_pos_old2 = robot_pos_old1;
robot_pos_old1 = robot_pos;
-
+
//CS ************ PD Stability CONTROLLER HERE ****************
error = target_angle - angle;
kp_term = Kp1 * error;
@@ -269,9 +358,9 @@
change_in_angle = angle - angle_old2; //add
kd_term = ((Kd1 * change_in_target_angle) - Kd1*(change_in_angle)) / dt;
//kd_term = ((Kd1 * change_in_target_angle) - (Kd1*velocity));
-
+
//pc.printf("dAngle:%f\r\n", angle-angle_old1);
-
+
//Control Output
control_output += kp_term + kd_term;
control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
@@ -294,88 +383,49 @@
} else { //[FALLEN}
//Disable Motors
enable = DISABLE;
-
+
//Set fallen flag
fallen = true;
}
- //Fast Loop
+ //Fast Loop
if(loop_counter >= 5) {
loop_counter = 0;
- pc.printf("angle:%d Kp1: %0.3f Kd1: %0.2f Kp2: %0.2f Kd2: %0.3f tang: %0.2f dt:%d rob_pos: %d V: %f\r\n", (int)angle, Kp1, Kd1, Kp2, Ki2, target_angle, dt, robot_pos, velocity);
+ pc.printf("angle:%d Kp1: %0.3f Kd1: %0.2f Kp2: %0.2f Kd2: %0.3f tang: %0.2f dt:%d rob_pos: %d VE: %f\r\n", (int)angle, Kp1, Kd1, Kp2, Ki2, target_angle, dt, robot_pos, velocity_error);
//pc.printf("Jstick_h: %d Jstick_v: %d Knob1 %d Knob2 %d Knob3 %d Knob4 %d Button: %d\r\n", jstick_h, jstick_v, knob1, knob2, knob3, knob4, button);
}
//Meduim Loop
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
+ //Process data with our protocal
communication_protocal(rxLen);
}
-
+
} // End of medium loop
-
+
//Slow Loop
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
+ //checkpin.rise(&dmpDataReady);
+ // } //END WHILE
/********************* 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;
+ /********************* All IMU Handling DO NOT MODIFY *****************/
- //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
- } else {
- pc.printf("\t\t\t filtered angle \r\n");
- }
- }
- /********************* All IMU Handling DO NOT MODIFY *****************/
-
} //end main loop
} //End Main()
\ No newline at end of file