Carter Sharer / Mbed 2 deprecated BroBot_RTOS_ESE350

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU6050_V3 mbed-rtos mbed

Fork of BroBot_RTOS_Development by Arvind Ramesh

main.cpp@8:8389c0a9339e, 2016-12-17 (annotated)

Committer:
csharer
Date:
Sat Dec 17 23:58:58 2016 +0000
Revision:
8:8389c0a9339e
Parent:
6:62cdb7482b50
Child:
9:fb671fa0c0be
Got the D in pos working well

Who changed what in which revision?

UserRevisionLine numberNew contents of line
csharer 4:2512939c10f0 1 //BroBot V3
csharer 4:2512939c10f0 2 //Author: Carter Sharer
csharer 4:2512939c10f0 3 //Date: 10/13/2016
csharer 6:62cdb7482b50 4 //Added communication protocol v1 (no type selection)
csharer 8:8389c0a9339e 5 //Jstick_v: 0 Knob1 39.898 Knob2 44.381 Knob3 10.55 Knob4 18.67 Button: 0
csharer 3:2f76ffbc5cef 6
csharer 4:2512939c10f0 7 //BroBot Begin
csharer 3:2f76ffbc5cef 8 #include "pin_assignments.h"
csharer 3:2f76ffbc5cef 9 #include "I2Cdev.h"
csharer 3:2f76ffbc5cef 10 #include "JJ_MPU6050_DMP_6Axis.h"
csharer 3:2f76ffbc5cef 11 #include "BroBot.h"
csharer 3:2f76ffbc5cef 12 #include "BroBot_IMU.h"
csharer 3:2f76ffbc5cef 13 #include "stepper_motors.h"
csharer 4:2512939c10f0 14 #include "MRF24J40.h"
csharer 4:2512939c10f0 15
csharer 6:62cdb7482b50 16 //Angle Offset is used to set the natural balance point of your robot.
csharer 6:62cdb7482b50 17 //You should adjust this offset so that your robots balance points is near 0
csharer 6:62cdb7482b50 18 #define ANGLE_OFFSET 107
csharer 6:62cdb7482b50 19
csharer 4:2512939c10f0 20 //For RF Communication
csharer 4:2512939c10f0 21 #define JSTICK_H 8
csharer 4:2512939c10f0 22 #define JSTICK_V 9
csharer 4:2512939c10f0 23 #define SPACE 10
csharer 4:2512939c10f0 24 #define KNOB1 11
csharer 4:2512939c10f0 25 #define KNOB2 12
csharer 4:2512939c10f0 26 #define KNOB3 13
csharer 4:2512939c10f0 27 #define KNOB4 14
csharer 4:2512939c10f0 28 #define ANGLE 15
csharer 4:2512939c10f0 29 #define BUTTON 16
csharer 4:2512939c10f0 30 #define JSTICK_OFFSET 100
csharer 4:2512939c10f0 31 #define TX_BUFFER_LEN 18
csharer 4:2512939c10f0 32 #define TX_ANGLE_OFFSET 100
csharer 4:2512939c10f0 33 //Knobs
csharer 4:2512939c10f0 34 #define POT1 p17
csharer 4:2512939c10f0 35 #define POT2 p18
csharer 4:2512939c10f0 36 #define POT3 p16
csharer 4:2512939c10f0 37 #define POT4 p15
csharer 4:2512939c10f0 38 //JoyStick
csharer 4:2512939c10f0 39 #define POTV p19
csharer 4:2512939c10f0 40 #define POTH p20
csharer 3:2f76ffbc5cef 41
csharer 3:2f76ffbc5cef 42 //PID
csharer 3:2f76ffbc5cef 43 #define MAX_THROTTLE 580
csharer 3:2f76ffbc5cef 44 #define MAX_STEERING 150
csharer 3:2f76ffbc5cef 45 #define MAX_TARGET_ANGLE 12
csharer 3:2f76ffbc5cef 46 #define KP 0.19
csharer 3:2f76ffbc5cef 47 #define KD 28
csharer 3:2f76ffbc5cef 48 #define KP_THROTTLE 0.01 //0.07
csharer 3:2f76ffbc5cef 49 #define KI_THROTTLE 0//0.04
csharer 3:2f76ffbc5cef 50 #define ITERM_MAX_ERROR 25 // Iterm windup constants for PI control //40
csharer 3:2f76ffbc5cef 51 #define ITERM_MAX 8000 // 5000
csharer 3:2f76ffbc5cef 52
csharer 4:2512939c10f0 53 //Controller Values
csharer 6:62cdb7482b50 54 float knob1, knob2, knob3, knob4;
csharer 6:62cdb7482b50 55 float jstick_h, jstick_v;
csharer 4:2512939c10f0 56
csharer 3:2f76ffbc5cef 57 //PID Default control values from constant definitions
csharer 6:62cdb7482b50 58 float Kp1 = KP;
csharer 6:62cdb7482b50 59 float Kd1 = KD;
csharer 6:62cdb7482b50 60 float Kp2 = KP_THROTTLE;
csharer 6:62cdb7482b50 61 float Ki2 = KI_THROTTLE;
csharer 6:62cdb7482b50 62 float Kd2; //Added for CS Pos contorl
csharer 3:2f76ffbc5cef 63 float PID_errorSum;
csharer 3:2f76ffbc5cef 64 float PID_errorOld = 0;
csharer 3:2f76ffbc5cef 65 float PID_errorOld2 = 0;
csharer 3:2f76ffbc5cef 66 float setPointOld = 0;
csharer 3:2f76ffbc5cef 67 float target_angle;
csharer 3:2f76ffbc5cef 68 float throttle = 0;
csharer 3:2f76ffbc5cef 69 float steering = 0;
csharer 3:2f76ffbc5cef 70 float max_throttle = MAX_THROTTLE;
csharer 3:2f76ffbc5cef 71 float max_steering = MAX_STEERING;
csharer 3:2f76ffbc5cef 72 float max_target_angle = MAX_TARGET_ANGLE;
csharer 3:2f76ffbc5cef 73 float control_output;
csharer 3:2f76ffbc5cef 74 int16_t actual_robot_speed; // overall robot speed (measured from steppers speed)
csharer 3:2f76ffbc5cef 75 int16_t actual_robot_speed_old;
csharer 3:2f76ffbc5cef 76 float estimated_speed_filtered; // Estimated robot speed
csharer 4:2512939c10f0 77 int robot_pos = 0;
csharer 3:2f76ffbc5cef 78
csharer 3:2f76ffbc5cef 79 Timer timer;
csharer 3:2f76ffbc5cef 80 int timer_value; //maybe make this a long
csharer 3:2f76ffbc5cef 81 int timer_old; //maybe make this a long
csharer 4:2512939c10f0 82 int dt;
csharer 3:2f76ffbc5cef 83
csharer 3:2f76ffbc5cef 84 uint8_t slow_loop_counter;
csharer 4:2512939c10f0 85 uint8_t medium_loop_counter;
csharer 3:2f76ffbc5cef 86 uint8_t loop_counter;
csharer 3:2f76ffbc5cef 87
csharer 3:2f76ffbc5cef 88 Serial pc(USBTX, USBRX);
csharer 3:2f76ffbc5cef 89
csharer 4:2512939c10f0 90 // LEDs
csharer 4:2512939c10f0 91 DigitalOut led1(LED1);
csharer 4:2512939c10f0 92 DigitalOut led2(LED2);
csharer 4:2512939c10f0 93 DigitalOut led3(LED3);
csharer 4:2512939c10f0 94 DigitalOut led4(LED4);
csharer 4:2512939c10f0 95
csharer 4:2512939c10f0 96 //Button
csharer 4:2512939c10f0 97 bool button;
csharer 6:62cdb7482b50 98 #include "communication.h"
csharer 4:2512939c10f0 99
csharer 3:2f76ffbc5cef 100 // ================================================================
csharer 3:2f76ffbc5cef 101 // === INITIAL SETUP ===
csharer 3:2f76ffbc5cef 102 // ================================================================
csharer 3:2f76ffbc5cef 103 void init_imu()
csharer 3:2f76ffbc5cef 104 {
csharer 3:2f76ffbc5cef 105 pc.printf("\r\r\n\n Start \r\n");
csharer 3:2f76ffbc5cef 106
csharer 3:2f76ffbc5cef 107 // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
csharer 3:2f76ffbc5cef 108 mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
csharer 3:2f76ffbc5cef 109 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
csharer 3:2f76ffbc5cef 110 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
csharer 3:2f76ffbc5cef 111 mpu.setDLPFMode(MPU6050_DLPF_BW_10); //10,20,42,98,188 // Default factor for BROBOT:10
csharer 4:2512939c10f0 112 mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
csharer 3:2f76ffbc5cef 113 mpu.setSleepEnabled(false);
csharer 3:2f76ffbc5cef 114 wait_ms(500);
csharer 3:2f76ffbc5cef 115
csharer 3:2f76ffbc5cef 116 // load and configure the DMP
csharer 3:2f76ffbc5cef 117 devStatus = mpu.dmpInitialize();
csharer 3:2f76ffbc5cef 118 if(devStatus == 0) {
csharer 3:2f76ffbc5cef 119 mpu.setDMPEnabled(true);
csharer 3:2f76ffbc5cef 120 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 121 dmpReady = true;
csharer 4:2512939c10f0 122 } else {
csharer 3:2f76ffbc5cef 123 // 1 = initial memory load failed
csharer 3:2f76ffbc5cef 124 // 2 = DMP configuration updates failed
csharer 3:2f76ffbc5cef 125 pc.printf("DMP INIT error \r\n");
csharer 3:2f76ffbc5cef 126 }
csharer 3:2f76ffbc5cef 127
csharer 3:2f76ffbc5cef 128 //Gyro Calibration
csharer 3:2f76ffbc5cef 129 wait_ms(500);
csharer 3:2f76ffbc5cef 130 pc.printf("Gyro calibration!! Dont move the robot in 10 seconds... \r\n");
csharer 3:2f76ffbc5cef 131 wait_ms(500);
csharer 4:2512939c10f0 132
csharer 3:2f76ffbc5cef 133 // verify connection
csharer 3:2f76ffbc5cef 134 pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");
csharer 3:2f76ffbc5cef 135
csharer 3:2f76ffbc5cef 136 //Adjust Sensor Fusion Gain
csharer 3:2f76ffbc5cef 137 dmpSetSensorFusionAccelGain(0x20);
csharer 3:2f76ffbc5cef 138
csharer 3:2f76ffbc5cef 139 wait_ms(200);
csharer 3:2f76ffbc5cef 140 mpu.resetFIFO();
csharer 3:2f76ffbc5cef 141 }
csharer 3:2f76ffbc5cef 142
csharer 3:2f76ffbc5cef 143 // ================================================================
csharer 3:2f76ffbc5cef 144 // === MAIN PROGRAM LOOP ===
csharer 3:2f76ffbc5cef 145 // ================================================================
csharer 4:2512939c10f0 146 //CS PID CONTROLLER TEST
csharer 4:2512939c10f0 147 float target_angle_old = 0;
csharer 4:2512939c10f0 148 float change_in_target_angle = 0;
csharer 4:2512939c10f0 149 float change_in_angle = 0;
csharer 4:2512939c10f0 150 float angle_old1 = 0;
csharer 4:2512939c10f0 151 float angle_old2 = 0;
csharer 4:2512939c10f0 152 float kp_term = 0;
csharer 4:2512939c10f0 153 float kd_term = 0;
csharer 4:2512939c10f0 154 float error;
csharer 4:2512939c10f0 155 //For Position controller
csharer 4:2512939c10f0 156 float pos_error = 0;
csharer 4:2512939c10f0 157 float kp_pos_term = 0;
csharer 4:2512939c10f0 158 float kd_pos_term = 0;
csharer 4:2512939c10f0 159 float change_in_target_pos;
csharer 4:2512939c10f0 160 float target_pos, target_pos_old;
csharer 4:2512939c10f0 161 float change_in_pos;
csharer 8:8389c0a9339e 162 float robot_pos_old1, robot_pos_old2;
csharer 8:8389c0a9339e 163 float velocity;
csharer 4:2512939c10f0 164
csharer 6:62cdb7482b50 165 bool fallen = true;
csharer 3:2f76ffbc5cef 166 int main()
csharer 3:2f76ffbc5cef 167 {
csharer 6:62cdb7482b50 168 //Set the Channel. 0 is default, 15 is max
csharer 6:62cdb7482b50 169 uint8_t channel = 2;
csharer 6:62cdb7482b50 170 mrf.SetChannel(channel);
csharer 6:62cdb7482b50 171
csharer 6:62cdb7482b50 172 pc.baud(115200);
csharer 3:2f76ffbc5cef 173 pc.printf("Start\r\n");
csharer 3:2f76ffbc5cef 174 init_imu();
csharer 3:2f76ffbc5cef 175 timer.start();
csharer 3:2f76ffbc5cef 176 //timer
csharer 4:2512939c10f0 177 timer_value = timer.read_us();
syundo0730 0:8d2c753a96e7 178
csharer 3:2f76ffbc5cef 179 //Init Stepper Motors
csharer 3:2f76ffbc5cef 180 //Attach Timer Interupts (Tiker)
csharer 3:2f76ffbc5cef 181 timer_M1.attach_us(&ISR1, ZERO_SPEED);
csharer 3:2f76ffbc5cef 182 timer_M2.attach_us(&ISR2, ZERO_SPEED);
csharer 3:2f76ffbc5cef 183 step_M1 = 1;
csharer 3:2f76ffbc5cef 184 dir_M1 = 1;
csharer 6:62cdb7482b50 185 enable = DISABLE; //Disable Motors
csharer 4:2512939c10f0 186
csharer 4:2512939c10f0 187 //Attach Interupt for IMU
csharer 4:2512939c10f0 188 checkpin.rise(&dmpDataReady);
csharer 4:2512939c10f0 189
csharer 4:2512939c10f0 190 //Used to set angle upon startup, filter
csharer 4:2512939c10f0 191 bool FILTER_DISABLE = true;
csharer 4:2512939c10f0 192
csharer 6:62cdb7482b50 193 //Enable Motors
csharer 6:62cdb7482b50 194 enable = ENABLE;
csharer 6:62cdb7482b50 195
csharer 3:2f76ffbc5cef 196 while(1) {
csharer 6:62cdb7482b50 197 //led1 = led1^1;
csharer 6:62cdb7482b50 198 led4 = !fallen;
csharer 6:62cdb7482b50 199 led2 = button;
csharer 6:62cdb7482b50 200
csharer 6:62cdb7482b50 201 if(jstick_v > 80)
csharer 6:62cdb7482b50 202 led3 = 1;
csharer 6:62cdb7482b50 203 else
csharer 6:62cdb7482b50 204 led3 = 0;
csharer 6:62cdb7482b50 205
csharer 4:2512939c10f0 206 if(button) {
csharer 4:2512939c10f0 207 pos_M1 = 0;
csharer 4:2512939c10f0 208 pos_M2 = 0;
csharer 4:2512939c10f0 209 target_pos = 0;
csharer 6:62cdb7482b50 210 fallen = false;
csharer 4:2512939c10f0 211 }
csharer 4:2512939c10f0 212
csharer 6:62cdb7482b50 213 while(!mpuInterrupt) {
csharer 4:2512939c10f0 214 timer_value = timer.read_us();
csharer 4:2512939c10f0 215
csharer 4:2512939c10f0 216 //Set Gainz with knobs
csharer 6:62cdb7482b50 217 Kp1 = ((float)knob1) / 1000.0;
csharer 6:62cdb7482b50 218 Kd1 = ((float)knob2) / 1.0;
csharer 6:62cdb7482b50 219 Kp2 = ((float)knob3) / 1000.0;
csharer 6:62cdb7482b50 220 Kd2 = ((float)knob4) / 100.0;
csharer 4:2512939c10f0 221
csharer 4:2512939c10f0 222 //Joystick control
csharer 4:2512939c10f0 223 throttle = (float)jstick_v /10.0;
csharer 4:2512939c10f0 224 steering = (float)jstick_h / 10.0;
csharer 4:2512939c10f0 225
csharer 4:2512939c10f0 226 //Update Values
csharer 3:2f76ffbc5cef 227 loop_counter++;
csharer 3:2f76ffbc5cef 228 slow_loop_counter++;
csharer 4:2512939c10f0 229 medium_loop_counter++;
csharer 3:2f76ffbc5cef 230 dt = (timer_value - timer_old);
csharer 3:2f76ffbc5cef 231 timer_old = timer_value;
csharer 4:2512939c10f0 232 angle_old = angle;
csharer 3:2f76ffbc5cef 233
csharer 6:62cdb7482b50 234 // STANDING: Motor Control Enabled
csharer 6:62cdb7482b50 235 if(((angle < 45) && (angle > -45)) && (fallen == false)) {
csharer 3:2f76ffbc5cef 236
csharer 6:62cdb7482b50 237 //CS Pd Target Angle Contoller Goes Here
csharer 6:62cdb7482b50 238
csharer 6:62cdb7482b50 239 //Robot Position
csharer 6:62cdb7482b50 240 robot_pos = (pos_M1 + pos_M2) / 2;
csharer 6:62cdb7482b50 241 target_pos += throttle/2;
csharer 4:2512939c10f0 242
csharer 8:8389c0a9339e 243 velocity = motor1 + motor2 / 2;
csharer 8:8389c0a9339e 244
csharer 8:8389c0a9339e 245 //CS ***************** Position error *************************
csharer 4:2512939c10f0 246 pos_error = robot_pos - target_pos; //robot_pos - target_pos;
csharer 6:62cdb7482b50 247
csharer 6:62cdb7482b50 248 //KP Term
csharer 6:62cdb7482b50 249 kp_pos_term = -Kp2 * pos_error;
csharer 4:2512939c10f0 250
csharer 4:2512939c10f0 251 //KD Term
csharer 4:2512939c10f0 252 change_in_target_pos = target_pos - target_pos_old;
csharer 4:2512939c10f0 253 change_in_pos = robot_pos - robot_pos_old2;
csharer 8:8389c0a9339e 254 //kd_pos_term = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
csharer 8:8389c0a9339e 255 kd_pos_term = ((Kd2 * change_in_target_pos) + (Kd2*velocity));
csharer 4:2512939c10f0 256 target_angle = kp_pos_term + kd_pos_term;
csharer 4:2512939c10f0 257 target_angle = CAP(target_angle, MAX_TARGET_ANGLE);
csharer 4:2512939c10f0 258
csharer 4:2512939c10f0 259 //Update values
csharer 4:2512939c10f0 260 target_pos_old = target_pos;
csharer 4:2512939c10f0 261 robot_pos_old2 = robot_pos_old1;
csharer 8:8389c0a9339e 262 robot_pos_old1 = robot_pos;
csharer 4:2512939c10f0 263
csharer 8:8389c0a9339e 264 //CS ************ PD Stability CONTROLLER HERE ****************
csharer 4:2512939c10f0 265 error = target_angle - angle;
csharer 6:62cdb7482b50 266 kp_term = Kp1 * error;
csharer 4:2512939c10f0 267
csharer 4:2512939c10f0 268 change_in_target_angle = target_angle - target_angle_old; //add
csharer 4:2512939c10f0 269 change_in_angle = angle - angle_old2; //add
csharer 6:62cdb7482b50 270 kd_term = ((Kd1 * change_in_target_angle) - Kd1*(change_in_angle)) / dt;
csharer 8:8389c0a9339e 271 //kd_term = ((Kd1 * change_in_target_angle) - (Kd1*velocity));
csharer 8:8389c0a9339e 272
csharer 6:62cdb7482b50 273 //pc.printf("dAngle:%f\r\n", angle-angle_old1);
csharer 6:62cdb7482b50 274
csharer 4:2512939c10f0 275 //Control Output
csharer 4:2512939c10f0 276 control_output += kp_term + kd_term;
csharer 4:2512939c10f0 277 control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
csharer 6:62cdb7482b50 278 motor1 = (int16_t)(control_output + (steering));
csharer 6:62cdb7482b50 279 motor2 = (int16_t)(control_output - (steering));
csharer 4:2512939c10f0 280 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
csharer 4:2512939c10f0 281 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
csharer 4:2512939c10f0 282
csharer 4:2512939c10f0 283 //Update variables
csharer 4:2512939c10f0 284 target_angle_old = target_angle;
csharer 4:2512939c10f0 285 angle_old2 = angle_old1;
csharer 4:2512939c10f0 286 angle_old1 = angle;
csharer 4:2512939c10f0 287
csharer 4:2512939c10f0 288 //Enable Motors
csharer 4:2512939c10f0 289 enable = ENABLE;
csharer 4:2512939c10f0 290 setMotor1Speed(-motor1);
csharer 4:2512939c10f0 291 setMotor2Speed(-motor2);
csharer 4:2512939c10f0 292 robot_pos += (-motor1 + -motor2) / 2;
csharer 4:2512939c10f0 293 //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);
csharer 6:62cdb7482b50 294 } else { //[FALLEN}
csharer 4:2512939c10f0 295 //Disable Motors
csharer 4:2512939c10f0 296 enable = DISABLE;
csharer 6:62cdb7482b50 297
csharer 6:62cdb7482b50 298 //Set fallen flag
csharer 6:62cdb7482b50 299 fallen = true;
csharer 4:2512939c10f0 300 }
csharer 4:2512939c10f0 301
csharer 6:62cdb7482b50 302 //Fast Loop
csharer 4:2512939c10f0 303 if(loop_counter >= 5) {
csharer 4:2512939c10f0 304 loop_counter = 0;
csharer 8:8389c0a9339e 305 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);
csharer 6:62cdb7482b50 306 //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);
csharer 4:2512939c10f0 307 }
csharer 3:2f76ffbc5cef 308
csharer 4:2512939c10f0 309 //Meduim Loop
csharer 4:2512939c10f0 310 if (medium_loop_counter >= 10) {
csharer 4:2512939c10f0 311 medium_loop_counter = 0; // Read status
csharer 6:62cdb7482b50 312
csharer 4:2512939c10f0 313 //Recieve Data
csharer 6:62cdb7482b50 314 rxLen = rf_receive(rxBuffer, 128);
csharer 6:62cdb7482b50 315 if(rxLen > 0) {
csharer 6:62cdb7482b50 316 led1 = led1^1;
csharer 6:62cdb7482b50 317 //Process data with our protocal
csharer 6:62cdb7482b50 318 communication_protocal(rxLen);
csharer 4:2512939c10f0 319 }
csharer 6:62cdb7482b50 320
csharer 4:2512939c10f0 321 } // End of medium loop
csharer 4:2512939c10f0 322
csharer 4:2512939c10f0 323 //Slow Loop
csharer 4:2512939c10f0 324 if(slow_loop_counter >= 99) {
csharer 4:2512939c10f0 325 slow_loop_counter = 0;
csharer 6:62cdb7482b50 326
csharer 6:62cdb7482b50 327 /* Send Data To Controller goes here *
csharer 6:62cdb7482b50 328 * */
csharer 6:62cdb7482b50 329
csharer 4:2512939c10f0 330 } //End of Slow Loop
csharer 3:2f76ffbc5cef 331
csharer 4:2512939c10f0 332 //Reattach interupt
csharer 4:2512939c10f0 333 checkpin.rise(&dmpDataReady);
csharer 4:2512939c10f0 334 } //END WHILE
csharer 4:2512939c10f0 335
csharer 6:62cdb7482b50 336
csharer 6:62cdb7482b50 337 /********************* All IMU Handling DO NOT MODIFY *****************/
csharer 4:2512939c10f0 338 //Disable IRQ
csharer 4:2512939c10f0 339 checkpin.rise(NULL);
csharer 6:62cdb7482b50 340
csharer 6:62cdb7482b50 341 //reset interrupt flag and get INT_STATUS byte
csharer 3:2f76ffbc5cef 342 mpuInterrupt = false;
csharer 3:2f76ffbc5cef 343 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 344
csharer 6:62cdb7482b50 345 //get current FIFO count
csharer 3:2f76ffbc5cef 346 fifoCount = mpu.getFIFOCount();
csharer 3:2f76ffbc5cef 347
csharer 3:2f76ffbc5cef 348 // check for overflow (this should never happen unless our code is too inefficient)
csharer 3:2f76ffbc5cef 349 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
csharer 3:2f76ffbc5cef 350 // reset so we can continue cleanly
csharer 3:2f76ffbc5cef 351 mpu.resetFIFO();
csharer 4:2512939c10f0 352 pc.printf("FIFO overflow!");
csharer 4:2512939c10f0 353
csharer 6:62cdb7482b50 354 //otherwise, check for DMP data ready interrupt (this should happen frequently)
csharer 3:2f76ffbc5cef 355 } else if (mpuIntStatus & 0x02) {
csharer 6:62cdb7482b50 356 //wait for correct available data length, should be a VERY short wait
csharer 4:2512939c10f0 357 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
csharer 3:2f76ffbc5cef 358
csharer 6:62cdb7482b50 359 //read a packet from FIFO
csharer 3:2f76ffbc5cef 360 mpu.getFIFOBytes(fifoBuffer, packetSize);
csharer 3:2f76ffbc5cef 361
csharer 6:62cdb7482b50 362 //track FIFO count here in case there is > 1 packet available
csharer 6:62cdb7482b50 363 //(this lets us immediately read more without waiting for an interrupt)
csharer 3:2f76ffbc5cef 364 fifoCount -= packetSize;
csharer 3:2f76ffbc5cef 365
csharer 4:2512939c10f0 366 //Read new angle from IMU
csharer 4:2512939c10f0 367 new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
csharer 4:2512939c10f0 368 dAngle = new_angle - angle;
csharer 3:2f76ffbc5cef 369
csharer 4:2512939c10f0 370 //Filter out angle readings larger then MAX_ANGLE_DELTA
csharer 4:2512939c10f0 371 if( ((dAngle < 15) && (dAngle > -15)) || FILTER_DISABLE) {
csharer 4:2512939c10f0 372 angle = new_angle;
csharer 4:2512939c10f0 373 FILTER_DISABLE = false; //turn of filter disabler
csharer 4:2512939c10f0 374 } else {
csharer 4:2512939c10f0 375 pc.printf("\t\t\t filtered angle \r\n");
csharer 4:2512939c10f0 376 }
csharer 3:2f76ffbc5cef 377 }
csharer 6:62cdb7482b50 378 /********************* All IMU Handling DO NOT MODIFY *****************/
csharer 6:62cdb7482b50 379
csharer 3:2f76ffbc5cef 380 } //end main loop
csharer 3:2f76ffbc5cef 381 } //End Main()