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:
- 10:48f640a54401
- Parent:
- 9:fb671fa0c0be
- Child:
- 11:2553f5798f84
--- a/main.cpp Mon Jan 09 17:50:35 2017 +0000
+++ b/main.cpp Tue Jan 10 21:10:39 2017 +0000
@@ -15,6 +15,13 @@
* To reset the motor positions you must press the josystick button anytime.
******************************************************************************/
+//Controller Values
+float knob1, knob2, knob3, knob4;
+float jstick_h, jstick_v;
+
+//Button
+bool button;
+
//BroBot Begin
#include "cmsis_os.h"
#include "pin_assignments.h"
@@ -24,10 +31,12 @@
#include "BroBot_IMU.h"
#include "stepper_motors.h"
#include "MRF24J40.h"
+#include "communication.h"
//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
+#define MRF_CHANNEL 2
//For RF Communication
#define JSTICK_H 8
@@ -62,9 +71,22 @@
#define ITERM_MAX_ERROR 25 // Iterm windup constants for PI control //40
#define ITERM_MAX 8000 // 5000
-//Controller Values
-float knob1, knob2, knob3, knob4;
-float jstick_h, jstick_v;
+#define GPIOA p21
+#define GPIOB p22
+#define GPIOC p23
+#define GPIOD p24
+
+//*********** Thread Definations BEGIN ***********//
+void imu_update_thread(void const *args);
+osThreadDef(imu_update_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
+//*********** Thread Definations END *************//
+
+//*********** Local Function Definations BEGIN **************//
+void init_system();
+void init_imu();
+//*********** Local Function Definations END **************//
+
+
//PID Default control values from constant definitions
float Kp1 = KP;
@@ -106,27 +128,34 @@
DigitalOut led3(LED3);
DigitalOut led4(LED4);
-//Button
-bool button;
-#include "communication.h"
+//GPIO Pins
+DigitalOut gpioMonitorA(GPIOA);
+DigitalOut gpioMonitorB(GPIOB);
+DigitalOut gpioMonitorC(GPIOC);
+DigitalOut gpioMonitorD(GPIOD);
-//Used to set angle upon startup, filter
-bool FILTER_DISABLE = true;
+
+
+
+//Used to set angle upon startup
+bool initilizeAngle = true;
// ================================================================
-// === Threads ===
+// === IMU Thread ===
// ================================================================
-void led2_thread(void const *args)
+void imu_update_thread(void const *args)
{
while (true) {
- //if(mpuInterrupt) {
+ gpioMonitorA = 1;
+ if(mpuInterrupt){
+ mpuInterrupt = false;
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
@@ -136,8 +165,7 @@
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
- pc.printf("FIFO overflow!");
-
+ pc.printf("MPU 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
@@ -152,32 +180,35 @@
//Read new angle from IMU
new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
+ //Calculate the delta angle
dAngle = new_angle - angle;
//Filter out angle readings larger then MAX_ANGLE_DELTA
- if( ((dAngle < 15) && (dAngle > -15)) || FILTER_DISABLE) {
+ if(initilizeAngle) {
angle = new_angle;
- FILTER_DISABLE = false; //turn of filter disabler
+ initilizeAngle = false;
+ } else if( ((dAngle < 15) && (dAngle > -15))) {
+ angle = new_angle;
} else {
- pc.printf("\t\t\t filtered angle \r\n");
+ pc.printf("\t\t\t Delta Angle too Large: %d. Ignored \r\n",dAngle);
}
}
+ gpioMonitorA = 0;
/********************* All IMU Handling DO NOT MODIFY *****************/
- //}
+ }//End of if(mpuInterrupt) loop
+ osThreadYield(); // Yield to a running thread
+ }
+}
- osDelay(1);
- //pc.printf(" thread2 ");
- }
-}
-osThreadDef(led2_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
+
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void init_imu()
{
- pc.printf("\r\r\n\n Start \r\n");
+ pc.printf("Start IMU Initilization.. \r\n");
// Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
@@ -202,7 +233,7 @@
//Gyro Calibration
wait_ms(500);
- pc.printf("Gyro calibration!! Dont move the robot in 10 seconds... \r\n");
+ pc.printf("Gyro calibration!! Dont move the robot in 1 second... \r\n");
wait_ms(500);
// verify connection
@@ -215,6 +246,42 @@
mpu.resetFIFO();
}
+void init_system()
+{
+ initilizeAngle = true;
+ //Set the Channel. 0 is default, 15 is max
+ mrf.SetChannel(MRF_CHANNEL);
+ enable = DISABLE; //Disable Motors
+ pc.baud(115200);
+ pc.printf("Booting BroBot mbed RTOS..\r\n");
+
+
+ //Initilize the IMU
+ init_imu();
+ //Attach Interupt for IMU on rising edge of checkpin
+ checkpin.rise(&dmpDataReady);
+ pc.printf("IMU Initilized..\r\n");
+
+ //Init Stepper Motors
+ //Attach Motor Control Timer Call Back Functions
+ timer_M1.attach_us(&ISR1, ZERO_SPEED);//1s Period
+ timer_M2.attach_us(&ISR2, ZERO_SPEED);//1s Period
+ step_M1 = 1;
+ dir_M1 = 1;
+ pc.printf("Motors Initilized..\r\n");
+
+ //Timers initilized
+ timer.start();
+ timer_value = timer.read_us();
+
+ //Init GPIO Monitors
+ gpioMonitorA = 0;
+ gpioMonitorB = 0;
+ gpioMonitorC = 0;
+ gpioMonitorD = 0;
+
+}
+
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
@@ -240,38 +307,19 @@
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();
- timer.start();
- //timer
- timer_value = timer.read_us();
+
- //Init Stepper Motors
- //Attach Timer Interupts (Tiker)
- timer_M1.attach_us(&ISR1, ZERO_SPEED);
- timer_M2.attach_us(&ISR2, ZERO_SPEED);
- step_M1 = 1;
- dir_M1 = 1;
- enable = DISABLE; //Disable Motors
+ init_system();
+ enable = ENABLE; //Enable Motors
- //Attach Interupt for IMU
- checkpin.rise(&dmpDataReady);
-
- //Used to set angle upon startup, filter
- //bool FILTER_DISABLE = true;
-
- //Enable Motors
- enable = ENABLE;
-
- //Create Threads
- osThreadCreate(osThread(led2_thread), NULL);
+ //Create IMU Thread
+ osThreadCreate(osThread(imu_update_thread), NULL);
while(1) {
+
+ //Get the time stamp as soon as it enters the loop
+ timer_value = timer.read_us();
//led1 = led1^1;
led4 = !fallen;
led2 = button;
@@ -281,6 +329,7 @@
else
led3 = 0;
+ //Button Press on the remote initilizes the robot position.
if(button) {
pos_M1 = 0;
pos_M2 = 0;
@@ -288,144 +337,141 @@
fallen = false;
}
- // 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;
- //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;
- Kp2 = ((float)knob3) / 1000.0;
- Kd2 = ((float)knob4) / 100.0;
+ //Set Gainz with knobs
+ Kp1 = ((float)knob1) / 1000.0;
+ Kd1 = ((float)knob2) / 1.0;
+ Kp2 = ((float)knob3) / 1000.0;
+ Kd2 = ((float)knob4) / 100.0;
+
+ //Joystick control
+ throttle = (float)jstick_v /10.0;
+ steering = (float)jstick_h / 10.0;
- //Joystick control
- throttle = (float)jstick_v /10.0;
- steering = (float)jstick_h / 10.0;
+ //Increment the loop counters
+ loop_counter++;
+ slow_loop_counter++;
+ medium_loop_counter++;
- //Update Values
- loop_counter++;
- slow_loop_counter++;
- medium_loop_counter++;
- dt = (timer_value - timer_old);
- timer_old = timer_value;
- angle_old = angle;
+ //Calculate the delta time
+ dt = (timer_value - timer_old);
+ timer_old = timer_value;
+ angle_old = angle;
- // STANDING: Motor Control Enabled
- if(((angle < 45) && (angle > -45)) && (fallen == false)) {
+ // STANDING: Motor Control Enabled
+ //******************** PID Control BEGIN ********************** //
+ if(((angle < 45) && (angle > -45)) && (fallen == false)) {
+ gpioMonitorB = 1;
- //CS Pd Target Angle Contoller Goes Here
+ //CS Pd Target Angle Contoller Goes Here
- //Robot Position
- robot_pos = (pos_M1 + pos_M2) / 2;
- target_pos += throttle/2;
+ //Robot Position
+ robot_pos = (pos_M1 + pos_M2) / 2;
+ target_pos += throttle/2;
- //Robot Current Velocity
- velocity = motor1 + motor2 / 2;
+ //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;
+ //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_pos_term = -Kp2 * pos_error;
+ //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;
- //kd_pos_term = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
- 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);
+ //KD Term
+ change_in_target_pos = target_pos - target_pos_old;
+ change_in_pos = robot_pos - robot_pos_old2;
+ //kd_pos_term = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
+ 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;
+ //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;
+ //CS ************ PD Stability CONTROLLER HERE ****************
+ error = target_angle - angle;
+ kp_term = Kp1 * error;
- change_in_target_angle = target_angle - target_angle_old; //add
- 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));
+ change_in_target_angle = target_angle - target_angle_old; //add
+ 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);
+ //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
- motor1 = (int16_t)(control_output + (steering));
- motor2 = (int16_t)(control_output - (steering));
- motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
- motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
+ //Control Output
+ control_output += kp_term + kd_term;
+ control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
+ motor1 = (int16_t)(control_output + (steering));
+ motor2 = (int16_t)(control_output - (steering));
+ motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
+ motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
- //Update variables
- target_angle_old = target_angle;
- angle_old2 = angle_old1;
- angle_old1 = angle;
+ //Update variables
+ target_angle_old = target_angle;
+ angle_old2 = angle_old1;
+ angle_old1 = angle;
- //Enable Motors
- enable = ENABLE;
- setMotor1Speed(-motor1);
- setMotor2Speed(-motor2);
- robot_pos += (-motor1 + -motor2) / 2;
- //pc.printf("m1: %d m2: %d angle: %0.1f, controlout: %f tAngle: %f dt: %f timer: %d \r\n", motor1, motor2, angle, control_output, target_angle, dt, timer_value);
- } else { //[FALLEN}
- //Disable Motors
- enable = DISABLE;
-
- //Set fallen flag
- fallen = true;
- }
+ //Enable Motors
+ enable = ENABLE;
+ setMotor1Speed(-motor1);
+ setMotor2Speed(-motor2);
+ robot_pos += (-motor1 + -motor2) / 2;
+ gpioMonitorB = 0;
+ //pc.printf("m1: %d m2: %d angle: %0.1f, controlout: %f tAngle: %f dt: %f timer: %d \r\n", motor1, motor2, angle, control_output, target_angle, dt, timer_value);
+ } else { //[FALLEN}
+ //Disable Motors
+ enable = DISABLE;
- //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 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);
- }
+ //Set fallen flag
+ fallen = true;
+ }
+ //******************** PID Control END ********************** //
- //Meduim Loop
- if (medium_loop_counter >= 10) {
- medium_loop_counter = 0; // Read status
+ //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 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);
+ }
- //Recieve Data
- rxLen = rf_receive(rxBuffer, 128);
- if(rxLen > 0) {
- led1 = led1^1;
- //Process data with our protocal
- communication_protocal(rxLen);
- }
+ //Meduim Loop
+ if (medium_loop_counter >= 10) {
+ medium_loop_counter = 0; // Read status
- } // End of medium loop
-
- //Slow Loop
- if(slow_loop_counter >= 99) {
- slow_loop_counter = 0;
-
- /* Send Data To Controller goes here *
- * */
+ //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 Slow Loop
-
- //Reattach interupt
- //checkpin.rise(&dmpDataReady);
- // } //END WHILE
+ //Slow Loop
+ if(slow_loop_counter >= 99) {
+ slow_loop_counter = 0;
-
- /********************* All IMU Handling DO NOT MODIFY *****************/
+ /* Send Data To Controller goes here *
+ * */
+ } //End of Slow Loop
- /********************* All IMU Handling DO NOT MODIFY *****************/
-
+ //Reattach interupt
+ //checkpin.rise(&dmpDataReady);
+
+ gpioMonitorB = 0;
} //end main loop
} //End Main()
\ No newline at end of file