Carter Sharer
ESE519 Lab6 Part3
Dependencies: MPU6050_Lab6_Part3 mbed
Fork of
BroBot_v2
by 
Carter Sharer
Diff: main.cpp
- Revision:
- 4:2512939c10f0
- Parent:
- 3:2f76ffbc5cef
- Child:
- 6:ae3e6aefe908
--- a/main.cpp Wed Oct 12 05:04:10 2016 +0000
+++ b/main.cpp Tue Oct 18 20:44:21 2016 +0000
@@ -1,13 +1,37 @@
-//BroBot V2
-//Author: Carter Sharer
-//Date: 10/11/2016
+//BroBot V3
+//Author: Carter Sharer
+//Date: 10/13/2016
+//BroBot Begin
#include "pin_assignments.h"
#include "I2Cdev.h"
#include "JJ_MPU6050_DMP_6Axis.h"
#include "BroBot.h"
#include "BroBot_IMU.h"
#include "stepper_motors.h"
+#include "MRF24J40.h"
+
+//For RF Communication
+#define JSTICK_H 8
+#define JSTICK_V 9
+#define SPACE 10
+#define KNOB1 11
+#define KNOB2 12
+#define KNOB3 13
+#define KNOB4 14
+#define ANGLE 15
+#define BUTTON 16
+#define JSTICK_OFFSET 100
+#define TX_BUFFER_LEN 18
+#define TX_ANGLE_OFFSET 100
+//Knobs
+#define POT1 p17
+#define POT2 p18
+#define POT3 p16
+#define POT4 p15
+//JoyStick
+#define POTV p19
+#define POTH p20
//PID
#define MAX_THROTTLE 580
@@ -20,11 +44,29 @@
#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;
+
+//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;
@@ -45,17 +87,29 @@
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;
Timer timer;
int timer_value; //maybe make this a long
int timer_old; //maybe make this a long
-float dt;
+int dt;
uint8_t slow_loop_counter;
+uint8_t medium_loop_counter;
uint8_t loop_counter;
+
Serial pc(USBTX, USBRX);
+// LEDs
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+//Button
+bool button;
+
// =============================================================================
// === PD controller implementation(Proportional, derivative) ===
// =============================================================================
@@ -68,15 +122,18 @@
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;
- //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
- //PID_errorOld2 = PID_errorOld;
- //PID_errorOld = input; // error for Kd is only the input component
- //setPointOld = setPoint;
+ output = Kp * error; //+ (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;
+
+ 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
@@ -106,7 +163,7 @@
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
+ mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
mpu.setSleepEnabled(false);
wait_ms(500);
@@ -116,7 +173,7 @@
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
- } else {
+ } else {
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
pc.printf("DMP INIT error \r\n");
@@ -126,7 +183,7 @@
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");
@@ -140,14 +197,32 @@
// ================================================================
// === 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(115200);
+ pc.baud(230400);
pc.printf("Start\r\n");
init_imu();
timer.start();
//timer
- timer_value = timer.read_ms();
+ timer_value = timer.read_us();
//Init Stepper Motors
//Attach Timer Interupts (Tiker)
@@ -160,50 +235,174 @@
//Set Gains
Kp_thr = 0; //0.15;
Ki_thr = 0; //0.15;
-
+
+ //Attach Interupt for IMU
+ checkpin.rise(&dmpDataReady);
+
+ //Used to set angle upon startup, filter
+ bool FILTER_DISABLE = true;
+
while(1) {
- // New DMP Orientation solution?
- fifoCount = mpu.getFIFOCount();
- if (fifoCount >= 18) {
- if (fifoCount > 18) { // If we have more than one packet we take the easy path: discard the buffer and wait for the next one
- pc.printf("FIFO RESET!!\r\n");
- mpu.resetFIFO();
- return;
- }
+
+ if(button) {
+ pos_M1 = 0;
+ pos_M2 = 0;
+ target_pos = 0;
+ }
+
+ while(!mpuInterrupt) { // && fifoCount < packetSize) {
+ //led4 = led4^1;
+ //pc.printf("In while comp loop \r\n");
+ 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;
+
+ //Joystick control
+ throttle = (float)jstick_v /10.0;
+ steering = (float)jstick_h / 10.0;
+
+ //Update Values
loop_counter++;
slow_loop_counter++;
+ medium_loop_counter++;
dt = (timer_value - timer_old);
timer_old = timer_value;
+ angle_old = angle;
- angle_old = angle;
- // Get new orientation angle from IMU (MPU6050)
- angle = dmpGetPhi();
+ // Motor contorl
+ if((angle < 45) && (angle > -45)) {
- mpu.resetFIFO(); // We always reset FIFO
- } // End of new IMU data
+ //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.
- if(loop_counter >= 5) {
- loop_counter = 0;
- int16_t offset =
- pc.printf("angle: %d \r\n", int16_t(angle-ANGLE_OFFSET));
- setMotor1Speed(int16_t(angle-ANGLE_OFFSET));
- setMotor2Speed(int16_t(angle-ANGLE_OFFSET));
- }
- if (slow_loop_counter >= 99) { // 2Hz
- slow_loop_counter = 0; // Read status
- } // End of slow loop
+ //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);
+
+ //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);
+
+ //Update values
+ target_pos_old = target_pos;
+ robot_pos_old2 = robot_pos_old1;
+ robot_pos_old1 = robot_pos_old;
+
+ //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));
+ 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;
+
+ //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 {
+ //Disable Motors
+ enable = DISABLE;
+ //Set Motor Speed 0
+ PID_errorSum = 0; // Reset PID I term
+ }
+
+ //Fast Loop
+ 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);
+ }
- /*
- //Set Gains
- Kp = 0.02;
- Kd = 0.01;
+ //Meduim Loop
+ if (medium_loop_counter >= 10) {
+ medium_loop_counter = 0; // Read status
+ led2 = led2^1;
- timer_value = timer.read_ms();
+ //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();
+ }
+ } // End of medium loop
+
+ //Slow Loop
+ if(slow_loop_counter >= 99) {
+ slow_loop_counter = 0;
- // if programming failed, don't try to do anything
- if (!dmpReady) return;
+ //Send Data
+ txBuffer[ANGLE] = (uint8_t)(angle + TX_ANGLE_OFFSET);
+ mrf.Send(txBuffer, TX_BUFFER_LEN);
+ } //End of Slow Loop
+ //Reattach interupt
+ checkpin.rise(&dmpDataReady);
+ } //END WHILE
+
+ //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
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
@@ -215,90 +414,34 @@
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
- //CS while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+ while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
- angle_old = angle; //Update old angle before reading new angle
-
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- angle = atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD;
- angle = angle - ANGLE_OFFSET;
- //pc.printf("angle: %f \r\n", angle);
-
- //Update timer
- dt = (timer_value - timer_old);
- timer_old = timer_value;
-
- //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)
- //CS actual_robot_speed_old = actual_robot_speed;
- //CS actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward
- //CS int16_t angular_velocity = (angle - angle_old) * 90.0; // 90 is an empirical extracted factor to adjust for real units
- //CS int16_t estimated_speed = -actual_robot_speed_old - angular_velocity; // We use robot_speed(t-1) or (t-2) to compensate the delay
- //CS 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);
- //CD target_angle = constrain(target_angle, -max_target_angle, max_target_angle); // limited output
- // 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.
-
- //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);
- control_output = constrain(control_output, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); // Limit max output from control
- motor1 = control_output + steering;
- motor2 = control_output - steering;
+ //Read new angle from IMU
+ new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
+ dAngle = new_angle - angle;
- //TEST P CONTROL
- float gain = 1;
- motor1 = angle * gain;
- motor2 = angle * gain;
- pc.printf("motor: %d control output: %f \r\n", motor1, control_output);
-
- // Limit max speed (control output)
- motor1 = constrain(motor1, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
- motor2 = constrain(motor2, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
-
+ //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
}
-
-
- // Put all the pid loop stuff here
- if((angle < 45) && (angle > -45)) {
- //Enable Motors
- if(motor1 == 0) {
- enable = DISABLE;
- setMotor1Speed(0);
- setMotor2Speed(0);
- } else {
- enable = ENABLE;
- setMotor1Speed(motor1);
- setMotor2Speed(motor2);
- }
- //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 {
- //Disable Motors
- enable = DISABLE;
- //Set Motor Speed 0
- PID_errorSum = 0; // Reset PID I term
- Kp = 0;
- Kd = 0;
- Kp_thr = 0;
- Ki_thr = 0;
- }
-
- *////////////
} //end main loop
} //End Main()
\ No newline at end of file