Britney Dorval
Final project with problem
Fork of
Yusheng-final_project
by 
Carter Sharer
Diff: main.cpp
- Revision:
- 6:ae3e6aefe908
- Parent:
- 4:2512939c10f0
- Child:
- 9:a8fd0bd49279
--- a/main.cpp Tue Oct 18 20:46:01 2016 +0000
+++ b/main.cpp Thu Nov 10 19:20:55 2016 +0000
@@ -1,16 +1,30 @@
-//BroBot V3
+//Balance Bot V4
//Author: Carter Sharer
//Date: 10/13/2016
+//ESE519 Lab 6 Part 3 Skeleton Code
-//BroBot Begin
+/******************************* 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.
+******************************************************************************/
+
+//Balance Bot Begin
#include "pin_assignments.h"
#include "I2Cdev.h"
#include "JJ_MPU6050_DMP_6Axis.h"
-#include "BroBot.h"
-#include "BroBot_IMU.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
@@ -34,71 +48,36 @@
#define POTH p20
//PID
-#define MAX_THROTTLE 580
-#define MAX_STEERING 150
+#define MAX_THROTTLE 100
#define MAX_TARGET_ANGLE 12
-#define KP 0.19
-#define KD 28
-#define KP_THROTTLE 0.01 //0.07
-#define KI_THROTTLE 0//0.04
-#define ITERM_MAX_ERROR 25 // Iterm windup constants for PI control //40
-#define ITERM_MAX 8000 // 5000
-
-//MRF24J40
-PinName mosi(SDI); //SDI
-PinName miso(SDO); //SDO
-PinName sck(SCK); //SCK
-PinName cs(CS); //CS
-PinName reset(RESET); //RESET
-// RF tranceiver to link with handheld.
-MRF24J40 mrf(mosi, miso, sck, cs, reset);
-uint8_t txBuffer[128]= {1, 8, 0, 0xA1, 0xB2, 0xC3, 0xD4, 0x00};
-uint8_t rxBuffer[128];
-uint8_t rxLen;
+//PID Default control values from constant definitions
+float Kp1;
+float Kd1;
+float Kp2;
+float Kd2;
//Controller Values
uint8_t knob1, knob2, knob3, knob4;
int8_t jstick_h, jstick_v;
-
-//PID Default control values from constant definitions
-float Kp = KP;
-float Kd = KD;
-float Kp_thr = KP_THROTTLE;
-float Ki_thr = KI_THROTTLE;
-float Kd_thr; //Added for CS Pos contorl
-float Kp_user = KP;
-float Kd_user = KD;
-float Kp_thr_user = KP_THROTTLE;
-float Ki_thr_user = KI_THROTTLE;
-bool newControlParameters = false;
-bool modifing_control_parameters = false;
-float PID_errorSum;
-float PID_errorOld = 0;
-float PID_errorOld2 = 0;
-float setPointOld = 0;
+//Control Variables
float target_angle;
-float throttle = 0;
-float steering = 0;
-float max_throttle = MAX_THROTTLE;
-float max_steering = MAX_STEERING;
+float throttle = 0; //From joystick
+float steering = 0; //From joystick
float max_target_angle = MAX_TARGET_ANGLE;
-float control_output;
-int16_t actual_robot_speed; // overall robot speed (measured from steppers speed)
-int16_t actual_robot_speed_old;
-float estimated_speed_filtered; // Estimated robot speed
-int robot_pos = 0;
+int robot_pos = 0; //Robots position
+bool fallen = true;
Timer timer;
-int timer_value; //maybe make this a long
-int timer_old; //maybe make this a long
+int timer_value;
+int timer_old;
int dt;
+//Loop Counters
uint8_t slow_loop_counter;
uint8_t medium_loop_counter;
uint8_t loop_counter;
-
Serial pc(USBTX, USBRX);
// LEDs
@@ -109,48 +88,8 @@
//Button
bool button;
-
-// =============================================================================
-// === PD controller implementation(Proportional, derivative) ===
-// =============================================================================
-// PD controller implementation(Proportional, derivative). DT is in miliseconds
-// stabilityPDControl( dt, angle, target_angle, Kp, Kd);
-float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd)
-{
- float error;
- float output;
-
- error = setPoint - input;
-
-
- // Kd is implemented in two parts
- // The biggest one using only the input (sensor) part not the SetPoint input-input(t-2)
- // And the second using the setpoint to make it a bit more agressive setPoint-setPoint(t-1)
- output = Kp * error; //+ (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;
+#include "communication.h"
- PID_errorOld2 = PID_errorOld;
- PID_errorOld = input; // error for Kd is only the input component
- setPointOld = setPoint;
- return output;
-
-
-}
-
-// PI controller implementation (Proportional, integral). DT is in miliseconds
-float speedPIControl(float DT, float input, float setPoint, float Kp, float Ki)
-{
- float error;
- float output;
-
- error = setPoint - input;
- PID_errorSum += CAP(error, ITERM_MAX_ERROR);
- PID_errorSum = CAP(PID_errorSum, ITERM_MAX);
-
- //Serial.println(PID_errorSum);
-
- output = Kp * error + Ki * PID_errorSum * DT * 0.001; // DT is in miliseconds...
- return (output);
-}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
@@ -197,27 +136,13 @@
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
-//CS PID CONTROLLER TEST
-float target_angle_old = 0;
-float change_in_target_angle = 0;
-float change_in_angle = 0;
-float angle_old1 = 0;
-float angle_old2 = 0;
-float kp_term = 0;
-float kd_term = 0;
-float error;
-//For Position controller
-float pos_error = 0;
-float kp_pos_term = 0;
-float kd_pos_term = 0;
-float change_in_target_pos;
-float target_pos, target_pos_old;
-float change_in_pos;
-float robot_pos_old, robot_pos_old1, robot_pos_old2;
-
int main()
{
- pc.baud(230400);
+ //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();
@@ -230,11 +155,7 @@
timer_M2.attach_us(&ISR2, ZERO_SPEED);
step_M1 = 1;
dir_M1 = 1;
- enable = ENABLE; //Enable Motors
-
- //Set Gains
- Kp_thr = 0; //0.15;
- Ki_thr = 0; //0.15;
+ enable = DISABLE; //Disable Motors
//Attach Interupt for IMU
checkpin.rise(&dmpDataReady);
@@ -242,172 +163,126 @@
//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 2 to indicate a button press
+ led2 = button;
+
+ //If button is pressed reset motor position
if(button) {
- pos_M1 = 0;
- pos_M2 = 0;
- target_pos = 0;
+ pos_M1 = 0; //Reset position of Motor 1
+ pos_M2 = 0; //Reset position of motor 2
+ fallen = false; //Reset fallen flag
}
- while(!mpuInterrupt) { // && fifoCount < packetSize) {
- //led4 = led4^1;
- //pc.printf("In while comp loop \r\n");
+ //This is the main while loop, all your code goes here
+ while(!mpuInterrupt) {
+ //Timer
timer_value = timer.read_us();
- //Set Gainz with knobs
- Kp = ((float)knob1) / 1000.0;
- Kd = ((float)knob2) / 1.0;
- Kp_thr = ((float)knob3) / 1000.0;
- Kd_thr = ((float)knob4) / 100.0;
+ //Set gainz with knobs
+ Kp1 = knob1;
+ Kd1 = knob2;
+ Kp2 = knob3;
+ Kd2 = knob4;
//Joystick control
- throttle = (float)jstick_v /10.0;
- steering = (float)jstick_h / 10.0;
+ throttle = jstick_v;
+ steering = jstick_h;
- //Update Values
+ /**** 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;
-
- // Motor contorl
- if((angle < 45) && (angle > -45)) {
+ /*****************************************/
- //PID CONTROL MAGIC GOES HERE
- // We calculate the estimated robot speed:
- // Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU)
- actual_robot_speed_old = actual_robot_speed;
- actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward
- int16_t angular_velocity = (angle - angle_old) * 90.0; // 90 is an empirical extracted factor to adjust for real units
- int16_t estimated_speed = -actual_robot_speed_old - angular_velocity; // We use robot_speed(t-1) or (t-2) to compensate the delay
- estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05; // low pass filter on estimated speed
- // SPEED CONTROL: This is a PI controller.
- // input:user throttle, variable: estimated robot speed, output: target robot angle to get the desired speed
- //CS target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
- //CS target_angle = CAP(target_angle, max_target_angle); // limited output
- //target_angle = 0;
- // Stability control: This is a PD controller.
- // input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed
- // We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed.
+ //STANDING: Motor Control Enabled
+ if(((angle < 45) && (angle > -45)) && (fallen == false)) {
- //pc.printf("dt: %f, angle: %f, target_angle: %f, Kp: %f, Kd: %f \r\n", dt, angle, target_angle, Kp, Kd);
- //control_output = stabilityPDControl(dt, angle, target_angle, Kp, Kd);
+ //Enable Motor
+ enable = ENABLE;
- //CS Pd Target Angle Contoller Goes Here
- target_pos += throttle;
- robot_pos = (pos_M1 + pos_M2) / 2;
- //KP Term
- pos_error = robot_pos - target_pos; //robot_pos - target_pos;
- kp_pos_term = -Kp_thr * pos_error;
-
- //KD Term
- change_in_target_pos = target_pos - target_pos_old;
- change_in_pos = robot_pos - robot_pos_old2;
- kd_pos_term = ((-Kd_thr * change_in_target_pos) - (-Kd_thr*change_in_pos)) /dt;
- target_angle = kp_pos_term + kd_pos_term;
- target_angle = CAP(target_angle, MAX_TARGET_ANGLE);
+ /* 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; */
- //Update values
- target_pos_old = target_pos;
- robot_pos_old2 = robot_pos_old1;
- robot_pos_old1 = robot_pos_old;
+ //Calculate motor inputs
+ motor1 = int16_t(throttle/2 + steering/8);
+ motor2 = int16_t(throttle/2 - steering/8);
- //CS PD Stability CONTROLLER HERE
- error = target_angle - angle;
- kp_term = Kp * error;
-
- change_in_target_angle = target_angle - target_angle_old; //add
- change_in_angle = angle - angle_old2; //add
- kd_term = ((Kd * change_in_target_angle) - Kd*(change_in_angle)) / dt;
-
- //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/4));
- motor2 = (int16_t)(control_output - (steering/4));
+ //Cap the max and min values [-100, 100]
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;
+
+ //Set Motor Speed here
+ setMotor1Speed(motor1);
+ setMotor2Speed(motor2);
- //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 {
+ } else { //FALLEN: Motor Control Disabled
//Disable Motors
enable = DISABLE;
- //Set Motor Speed 0
- PID_errorSum = 0; // Reset PID I term
+
+ //Set fallen flag
+ fallen = true;
}
-
- //Fast Loop
+
+ /* 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 horz: %d verti: %d knob1: %d knob2: %d knob3: %d knob4: %d \r\n", int16_t(angle-ANGLE_OFFSET), jstick_h, jstick_v, knob1, knob2, knob3, knob4);
- //setMotor1Speed(int16_t(angle));
- //setMotor2Speed(int16_t(angle));
- //pc.printf("horz: %d verti: %d knob1: %d angle: %d \r\n", jstick_h, jstick_v, knob1, (int)angle);
- //pc.printf("angle: %d \r\n", (int)angle);
- pc.printf("angle:%d Kp: %0.3f Kd: %0.2f Kp_thr: %0.2f Kd_thr: %0.3f tang: %0.2f dt:%d pos_M1:%d pos_M2:%d rob_pos: %d\r\n", (int)angle, Kp, Kd, Kp_thr, Ki_thr, target_angle, dt, pos_M1, pos_M2, robot_pos);
+ 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
+ //Meduim Loop: Good for sending and receiving
if (medium_loop_counter >= 10) {
medium_loop_counter = 0; // Read status
- led2 = led2^1;
//Recieve Data
- rxLen = mrf.Receive(rxBuffer, 128);
- if(rxLen) {
- if((rxBuffer[0] == (uint8_t)1) && (rxBuffer[1] == (uint8_t)8) && (rxBuffer[2]==(uint8_t)0)) {
- jstick_h = (int8_t)rxBuffer[JSTICK_H] - JSTICK_OFFSET;
- jstick_v = (int8_t)rxBuffer[JSTICK_V] - JSTICK_OFFSET;
- knob1 = rxBuffer[KNOB1];
- knob2 = rxBuffer[KNOB2];
- knob3 = rxBuffer[KNOB3];
- knob4 = rxBuffer[KNOB4];
- button = rxBuffer[BUTTON];
- led1= led1^1; //flash led for debuggin
- led4 = button;
- }
- } else {
- mrf.Reset();
+ rxLen = rf_receive(rxBuffer, 128);
+ if(rxLen > 0) {
+ led1 = led1^1;
+ //Process data with our protocal
+ communication_protocal(rxLen);
}
+
} // End of medium loop
-
- //Slow Loop
+
+ //Slow Loop: Good for sending if speed is not an issue
if(slow_loop_counter >= 99) {
slow_loop_counter = 0;
- //Send Data
- txBuffer[ANGLE] = (uint8_t)(angle + TX_ANGLE_OFFSET);
- mrf.Send(txBuffer, TX_BUFFER_LEN);
+ /* Send Data To Controller goes here *
+ * */
} //End of Slow Loop
//Reattach interupt
checkpin.rise(&dmpDataReady);
} //END WHILE
+
+ /********************* All IMU Handling DO NOT MODIFY *****************/
//Disable IRQ
checkpin.rise(NULL);
- led3 = led3^1;
- //pc.printf("taking care of imu stuff angle: %f \r\n", angle);
- //All IMU stuff
- // reset interrupt flag and get INT_STATUS byte
+
+ //reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
- // get current FIFO count
+ //get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
@@ -416,32 +291,29 @@
mpu.resetFIFO();
pc.printf("FIFO overflow!");
- // otherwise, check for DMP data ready interrupt (this should happen frequently)
+ //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
+ //wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
- // read a packet from FIFO
+ //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)
+ //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");
- }
- //END IMU STUFF
+ }
+ }
+ /********************* All IMU Handling DO NOT MODIFY *****************/
- }
} //end main loop
} //End Main()
\ No newline at end of file