Justin Liu / Mbed 2 deprecated robotcode

robot

Dependencies:   MPU6050_Lab6_Part3 mbed

Fork of ESE519_Lab6_part3_skeleton by Carter Sharer

main.cpp@9:d9776d8ce47c, 2016-11-16 (annotated)

Committer:
jliutang
Date:
Wed Nov 16 00:14:11 2016 +0000
Revision:
9:d9776d8ce47c
Parent:
6:ae3e6aefe908 
11/15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
csharer 6:ae3e6aefe908 1 //Balance Bot V4
csharer 4:2512939c10f0 2 //Author: Carter Sharer
csharer 4:2512939c10f0 3 //Date: 10/13/2016
csharer 6:ae3e6aefe908 4 //ESE519 Lab 6 Part 3 Skeleton Code
csharer 3:2f76ffbc5cef 5
csharer 6:ae3e6aefe908 6 /******************************* README USAGE *******************************
csharer 6:ae3e6aefe908 7 * This robot must be powered on while it is laying down flat on a still table
csharer 6:ae3e6aefe908 8 * This allows the robot to calibrate the IMU (~5 seconds)
csharer 6:ae3e6aefe908 9 * The motors are DISABLED when the robot tilts more then +-45 degrees from
csharer 6:ae3e6aefe908 10 * vertical. To ENABLE the motors you must lift the robot to < +- 45 degres and
csharer 6:ae3e6aefe908 11 * press the joystick button.
csharer 6:ae3e6aefe908 12 * To reset the motor positions you must press the josystick button anytime.
csharer 6:ae3e6aefe908 13 ******************************************************************************/
csharer 6:ae3e6aefe908 14
csharer 6:ae3e6aefe908 15 //Balance Bot Begin
csharer 3:2f76ffbc5cef 16 #include "pin_assignments.h"
csharer 3:2f76ffbc5cef 17 #include "I2Cdev.h"
csharer 3:2f76ffbc5cef 18 #include "JJ_MPU6050_DMP_6Axis.h"
csharer 6:ae3e6aefe908 19 #include "balance_bot.h"
csharer 6:ae3e6aefe908 20 #include "balance_bot_IMU.h"
csharer 3:2f76ffbc5cef 21 #include "stepper_motors.h"
csharer 4:2512939c10f0 22 #include "MRF24J40.h"
csharer 4:2512939c10f0 23
csharer 6:ae3e6aefe908 24 //Angle Offset is used to set the natural balance point of your robot.
csharer 6:ae3e6aefe908 25 //You should adjust this offset so that your robots balance point is near 0
csharer 6:ae3e6aefe908 26 #define ANGLE_OFFSET 107
csharer 6:ae3e6aefe908 27
csharer 4:2512939c10f0 28 //For RF Communication
csharer 4:2512939c10f0 29 #define JSTICK_H 8
csharer 4:2512939c10f0 30 #define JSTICK_V 9
csharer 4:2512939c10f0 31 #define SPACE 10
csharer 4:2512939c10f0 32 #define KNOB1 11
csharer 4:2512939c10f0 33 #define KNOB2 12
csharer 4:2512939c10f0 34 #define KNOB3 13
csharer 4:2512939c10f0 35 #define KNOB4 14
csharer 4:2512939c10f0 36 #define ANGLE 15
csharer 4:2512939c10f0 37 #define BUTTON 16
csharer 4:2512939c10f0 38 #define JSTICK_OFFSET 100
csharer 4:2512939c10f0 39 #define TX_BUFFER_LEN 18
csharer 4:2512939c10f0 40 #define TX_ANGLE_OFFSET 100
csharer 4:2512939c10f0 41 //Knobs
csharer 4:2512939c10f0 42 #define POT1 p17
csharer 4:2512939c10f0 43 #define POT2 p18
csharer 4:2512939c10f0 44 #define POT3 p16
csharer 4:2512939c10f0 45 #define POT4 p15
csharer 4:2512939c10f0 46 //JoyStick
csharer 4:2512939c10f0 47 #define POTV p19
csharer 4:2512939c10f0 48 #define POTH p20
csharer 3:2f76ffbc5cef 49
csharer 3:2f76ffbc5cef 50 //PID
csharer 6:ae3e6aefe908 51 #define MAX_THROTTLE 100
csharer 3:2f76ffbc5cef 52 #define MAX_TARGET_ANGLE 12
csharer 6:ae3e6aefe908 53 //PID Default control values from constant definitions
csharer 6:ae3e6aefe908 54 float Kp1;
csharer 6:ae3e6aefe908 55 float Kd1;
csharer 6:ae3e6aefe908 56 float Kp2;
csharer 6:ae3e6aefe908 57 float Kd2;
csharer 4:2512939c10f0 58
csharer 4:2512939c10f0 59 //Controller Values
csharer 4:2512939c10f0 60 uint8_t knob1, knob2, knob3, knob4;
csharer 4:2512939c10f0 61 int8_t jstick_h, jstick_v;
csharer 4:2512939c10f0 62
csharer 6:ae3e6aefe908 63 //Control Variables
csharer 3:2f76ffbc5cef 64 float target_angle;
csharer 6:ae3e6aefe908 65 float throttle = 0; //From joystick
csharer 6:ae3e6aefe908 66 float steering = 0; //From joystick
csharer 3:2f76ffbc5cef 67 float max_target_angle = MAX_TARGET_ANGLE;
csharer 6:ae3e6aefe908 68 int robot_pos = 0; //Robots position
csharer 6:ae3e6aefe908 69 bool fallen = true;
jliutang 9:d9776d8ce47c 70 float d_angle = 0;
jliutang 9:d9776d8ce47c 71 float d_anglemax = 0;
jliutang 9:d9776d8ce47c 72 float angle_old2 = 0;
jliutang 9:d9776d8ce47c 73 float angle_old3 = 0;
jliutang 9:d9776d8ce47c 74 float angle_correction = 0;
jliutang 9:d9776d8ce47c 75 float refAngle = 0;
csharer 3:2f76ffbc5cef 76
csharer 3:2f76ffbc5cef 77 Timer timer;
csharer 6:ae3e6aefe908 78 int timer_value;
csharer 6:ae3e6aefe908 79 int timer_old;
csharer 4:2512939c10f0 80 int dt;
csharer 3:2f76ffbc5cef 81
csharer 6:ae3e6aefe908 82 //Loop Counters
csharer 3:2f76ffbc5cef 83 uint8_t slow_loop_counter;
csharer 4:2512939c10f0 84 uint8_t medium_loop_counter;
csharer 3:2f76ffbc5cef 85 uint8_t loop_counter;
csharer 3:2f76ffbc5cef 86
csharer 3:2f76ffbc5cef 87 Serial pc(USBTX, USBRX);
csharer 3:2f76ffbc5cef 88
csharer 4:2512939c10f0 89 // LEDs
csharer 4:2512939c10f0 90 DigitalOut led1(LED1);
csharer 4:2512939c10f0 91 DigitalOut led2(LED2);
csharer 4:2512939c10f0 92 DigitalOut led3(LED3);
csharer 4:2512939c10f0 93 DigitalOut led4(LED4);
csharer 4:2512939c10f0 94
csharer 4:2512939c10f0 95 //Button
csharer 4:2512939c10f0 96 bool button;
csharer 6:ae3e6aefe908 97 #include "communication.h"
csharer 4:2512939c10f0 98
csharer 3:2f76ffbc5cef 99 // ================================================================
csharer 3:2f76ffbc5cef 100 // === INITIAL SETUP ===
csharer 3:2f76ffbc5cef 101 // ================================================================
csharer 3:2f76ffbc5cef 102 void init_imu()
csharer 3:2f76ffbc5cef 103 {
csharer 3:2f76ffbc5cef 104 pc.printf("\r\r\n\n Start \r\n");
csharer 3:2f76ffbc5cef 105
csharer 3:2f76ffbc5cef 106 // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
csharer 3:2f76ffbc5cef 107 mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
csharer 3:2f76ffbc5cef 108 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
csharer 3:2f76ffbc5cef 109 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
csharer 3:2f76ffbc5cef 110 mpu.setDLPFMode(MPU6050_DLPF_BW_10); //10,20,42,98,188 // Default factor for BROBOT:10
csharer 4:2512939c10f0 111 mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
csharer 3:2f76ffbc5cef 112 mpu.setSleepEnabled(false);
csharer 3:2f76ffbc5cef 113 wait_ms(500);
csharer 3:2f76ffbc5cef 114
csharer 3:2f76ffbc5cef 115 // load and configure the DMP
csharer 3:2f76ffbc5cef 116 devStatus = mpu.dmpInitialize();
csharer 3:2f76ffbc5cef 117 if(devStatus == 0) {
csharer 3:2f76ffbc5cef 118 mpu.setDMPEnabled(true);
csharer 3:2f76ffbc5cef 119 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 120 dmpReady = true;
csharer 4:2512939c10f0 121 } else {
csharer 3:2f76ffbc5cef 122 // 1 = initial memory load failed
csharer 3:2f76ffbc5cef 123 // 2 = DMP configuration updates failed
csharer 3:2f76ffbc5cef 124 pc.printf("DMP INIT error \r\n");
csharer 3:2f76ffbc5cef 125 }
csharer 3:2f76ffbc5cef 126
csharer 3:2f76ffbc5cef 127 //Gyro Calibration
csharer 3:2f76ffbc5cef 128 wait_ms(500);
csharer 3:2f76ffbc5cef 129 pc.printf("Gyro calibration!! Dont move the robot in 10 seconds... \r\n");
csharer 3:2f76ffbc5cef 130 wait_ms(500);
csharer 4:2512939c10f0 131
csharer 3:2f76ffbc5cef 132 // verify connection
csharer 3:2f76ffbc5cef 133 pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");
csharer 3:2f76ffbc5cef 134
csharer 3:2f76ffbc5cef 135 //Adjust Sensor Fusion Gain
csharer 3:2f76ffbc5cef 136 dmpSetSensorFusionAccelGain(0x20);
csharer 3:2f76ffbc5cef 137
csharer 3:2f76ffbc5cef 138 wait_ms(200);
csharer 3:2f76ffbc5cef 139 mpu.resetFIFO();
csharer 3:2f76ffbc5cef 140 }
csharer 3:2f76ffbc5cef 141
csharer 3:2f76ffbc5cef 142 // ================================================================
csharer 3:2f76ffbc5cef 143 // === MAIN PROGRAM LOOP ===
csharer 3:2f76ffbc5cef 144 // ================================================================
csharer 3:2f76ffbc5cef 145 int main()
csharer 3:2f76ffbc5cef 146 {
csharer 6:ae3e6aefe908 147 //Set the Channel. 0 is default, 15 is max
csharer 6:ae3e6aefe908 148 uint8_t channel = 2;
csharer 6:ae3e6aefe908 149 mrf.SetChannel(channel);
csharer 6:ae3e6aefe908 150
csharer 6:ae3e6aefe908 151 pc.baud(115200);
csharer 3:2f76ffbc5cef 152 pc.printf("Start\r\n");
csharer 3:2f76ffbc5cef 153 init_imu();
csharer 3:2f76ffbc5cef 154 timer.start();
csharer 3:2f76ffbc5cef 155 //timer
csharer 4:2512939c10f0 156 timer_value = timer.read_us();
syundo0730 0:8d2c753a96e7 157
csharer 3:2f76ffbc5cef 158 //Init Stepper Motors
csharer 3:2f76ffbc5cef 159 //Attach Timer Interupts (Tiker)
csharer 3:2f76ffbc5cef 160 timer_M1.attach_us(&ISR1, ZERO_SPEED);
csharer 3:2f76ffbc5cef 161 timer_M2.attach_us(&ISR2, ZERO_SPEED);
csharer 3:2f76ffbc5cef 162 step_M1 = 1;
csharer 3:2f76ffbc5cef 163 dir_M1 = 1;
csharer 6:ae3e6aefe908 164 enable = DISABLE; //Disable Motors
csharer 4:2512939c10f0 165
csharer 4:2512939c10f0 166 //Attach Interupt for IMU
csharer 4:2512939c10f0 167 checkpin.rise(&dmpDataReady);
csharer 4:2512939c10f0 168
csharer 4:2512939c10f0 169 //Used to set angle upon startup, filter
csharer 4:2512939c10f0 170 bool FILTER_DISABLE = true;
csharer 4:2512939c10f0 171
csharer 6:ae3e6aefe908 172 //Enable Motors
csharer 6:ae3e6aefe908 173 enable = ENABLE;
csharer 6:ae3e6aefe908 174
jliutang 9:d9776d8ce47c 175
jliutang 9:d9776d8ce47c 176 float integral1 = 0;
jliutang 9:d9776d8ce47c 177 float integral2 = 0;
jliutang 9:d9776d8ce47c 178 int16_t motor1Old = 0;
jliutang 9:d9776d8ce47c 179 int16_t motor2Old = 0;
jliutang 9:d9776d8ce47c 180 float lastError = 0;
csharer 3:2f76ffbc5cef 181 while(1) {
csharer 6:ae3e6aefe908 182 //Led 4 to indicate if robot it STANDING or FALLEN
csharer 6:ae3e6aefe908 183 led4 = !fallen;
csharer 4:2512939c10f0 184
csharer 6:ae3e6aefe908 185 //Led 2 to indicate a button press
csharer 6:ae3e6aefe908 186 led2 = button;
csharer 6:ae3e6aefe908 187
csharer 6:ae3e6aefe908 188 //If button is pressed reset motor position
csharer 4:2512939c10f0 189 if(button) {
csharer 6:ae3e6aefe908 190 pos_M1 = 0; //Reset position of Motor 1
csharer 6:ae3e6aefe908 191 pos_M2 = 0; //Reset position of motor 2
csharer 6:ae3e6aefe908 192 fallen = false; //Reset fallen flag
csharer 4:2512939c10f0 193 }
csharer 4:2512939c10f0 194
csharer 6:ae3e6aefe908 195 //This is the main while loop, all your code goes here
csharer 6:ae3e6aefe908 196 while(!mpuInterrupt) {
csharer 6:ae3e6aefe908 197 //Timer
csharer 4:2512939c10f0 198 timer_value = timer.read_us();
csharer 4:2512939c10f0 199
csharer 6:ae3e6aefe908 200 //Set gainz with knobs
csharer 6:ae3e6aefe908 201 Kp1 = knob1;
csharer 6:ae3e6aefe908 202 Kd1 = knob2;
csharer 6:ae3e6aefe908 203 Kp2 = knob3;
csharer 6:ae3e6aefe908 204 Kd2 = knob4;
csharer 4:2512939c10f0 205
csharer 4:2512939c10f0 206 //Joystick control
csharer 6:ae3e6aefe908 207 throttle = jstick_v;
csharer 6:ae3e6aefe908 208 steering = jstick_h;
csharer 4:2512939c10f0 209
csharer 3:2f76ffbc5cef 210
jliutang 9:d9776d8ce47c 211 float error = 0;
csharer 6:ae3e6aefe908 212 //STANDING: Motor Control Enabled
csharer 6:ae3e6aefe908 213 if(((angle < 45) && (angle > -45)) && (fallen == false)) {
csharer 3:2f76ffbc5cef 214
jliutang 9:d9776d8ce47c 215 //wait_ms(1);
csharer 6:ae3e6aefe908 216 //Enable Motor
csharer 6:ae3e6aefe908 217 enable = ENABLE;
csharer 4:2512939c10f0 218
jliutang 9:d9776d8ce47c 219 //controls
jliutang 9:d9776d8ce47c 220 error = angle - 0;//angle_correction; //should be balanced at 0
jliutang 9:d9776d8ce47c 221
jliutang 9:d9776d8ce47c 222 float Ki = 0.5;
jliutang 9:d9776d8ce47c 223
jliutang 9:d9776d8ce47c 224 d_angle = angle-(angle_old+angle_old2+angle_old3)/3.0;
jliutang 9:d9776d8ce47c 225
jliutang 9:d9776d8ce47c 226 /*
jliutang 9:d9776d8ce47c 227 if (d_angle > d_anglemax) d_anglemax = d_angle;
jliutang 9:d9776d8ce47c 228 if (d_angle > 2.0) d_angle = 2.0;
jliutang 9:d9776d8ce47c 229 if (d_angle < -2.0) d_angle = -2.0;*/
jliutang 9:d9776d8ce47c 230 if (d_angle < 3.0 && d_angle > -3.0) d_angle = 0;
jliutang 9:d9776d8ce47c 231
jliutang 9:d9776d8ce47c 232 motor1 = error * Kp1/10.0 + Kd1/25.0 * d_angle;
jliutang 9:d9776d8ce47c 233 motor2 = error * Kp2/10.0 + Kd2/25.0 * d_angle;
jliutang 9:d9776d8ce47c 234
jliutang 9:d9776d8ce47c 235 /*
jliutang 9:d9776d8ce47c 236 if (d_angle == 0){
jliutang 9:d9776d8ce47c 237 if (motor1 < -5) angle_correction += 0.002;
jliutang 9:d9776d8ce47c 238 else if (motor1 > 5) angle_correction -= 0.002;
jliutang 9:d9776d8ce47c 239 }
jliutang 9:d9776d8ce47c 240 */
jliutang 9:d9776d8ce47c 241 /*long positionError = pos_M1 - 0;
jliutang 9:d9776d8ce47c 242 refAngle -= (double)positionError/500000.0; //initially 0
jliutang 9:d9776d8ce47c 243
jliutang 9:d9776d8ce47c 244 refAngle -= (double)motor1Old/500000.0;
jliutang 9:d9776d8ce47c 245
jliutang 9:d9776d8ce47c 246 if (refAngle < -5) {
jliutang 9:d9776d8ce47c 247
jliutang 9:d9776d8ce47c 248 refAngle = -5;
jliutang 9:d9776d8ce47c 249
jliutang 9:d9776d8ce47c 250 }
jliutang 9:d9776d8ce47c 251 else if (refAngle > 5) {
jliutang 9:d9776d8ce47c 252
jliutang 9:d9776d8ce47c 253 refAngle = 5;
jliutang 9:d9776d8ce47c 254
jliutang 9:d9776d8ce47c 255 }
jliutang 9:d9776d8ce47c 256
jliutang 9:d9776d8ce47c 257 error = (angle - refAngle)/10.0;
jliutang 9:d9776d8ce47c 258 double P = Kp1 * error;
jliutang 9:d9776d8ce47c 259 //I += 0.0001 * error;
jliutang 9:d9776d8ce47c 260 double D = Kd1 * (error - lastError);
jliutang 9:d9776d8ce47c 261
jliutang 9:d9776d8ce47c 262 lastError = error;
jliutang 9:d9776d8ce47c 263 motor1 = error * P/10.0 + D/25.0 ;
jliutang 9:d9776d8ce47c 264 motor2 = error * P/10.0 + D/25.0 ;
jliutang 9:d9776d8ce47c 265 */
csharer 4:2512939c10f0 266
csharer 6:ae3e6aefe908 267 //Calculate motor inputs
jliutang 9:d9776d8ce47c 268 motor1 += 0.1 * int16_t(throttle/2 + steering/8);
jliutang 9:d9776d8ce47c 269 motor2 += 0.1 * int16_t(throttle/2 - steering/8);
csharer 4:2512939c10f0 270
csharer 6:ae3e6aefe908 271 //Cap the max and min values [-100, 100]
csharer 4:2512939c10f0 272 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
csharer 4:2512939c10f0 273 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
csharer 6:ae3e6aefe908 274
jliutang 9:d9776d8ce47c 275 //Update speed if motor changes are above threshold
jliutang 9:d9776d8ce47c 276 int thresh = 1;
jliutang 9:d9776d8ce47c 277 if ((motor1 - motor1Old >= thresh) || (motor1-motor1Old <= -thresh)) {
jliutang 9:d9776d8ce47c 278 setMotor1Speed(motor1);
jliutang 9:d9776d8ce47c 279 motor1Old = motor1;
jliutang 9:d9776d8ce47c 280 }
jliutang 9:d9776d8ce47c 281 //motor1Old = motor1;
jliutang 9:d9776d8ce47c 282 if ((motor2 - motor2Old >= thresh) || (motor2-motor2Old <= -thresh)) {
jliutang 9:d9776d8ce47c 283 setMotor2Speed(motor2);
jliutang 9:d9776d8ce47c 284 motor2Old = motor2;
jliutang 9:d9776d8ce47c 285 }
jliutang 9:d9776d8ce47c 286 //motor2Old = motor2;
csharer 6:ae3e6aefe908 287 //Set Motor Speed here
jliutang 9:d9776d8ce47c 288 //setMotor1Speed(motor1);
jliutang 9:d9776d8ce47c 289 //setMotor2Speed(motor2);
csharer 4:2512939c10f0 290
csharer 6:ae3e6aefe908 291 } else { //FALLEN: Motor Control Disabled
csharer 4:2512939c10f0 292 //Disable Motors
csharer 4:2512939c10f0 293 enable = DISABLE;
csharer 6:ae3e6aefe908 294
csharer 6:ae3e6aefe908 295 //Set fallen flag
csharer 6:ae3e6aefe908 296 fallen = true;
jliutang 9:d9776d8ce47c 297
jliutang 9:d9776d8ce47c 298 integral1 = 0;
jliutang 9:d9776d8ce47c 299 integral2 = 0;
jliutang 9:d9776d8ce47c 300 angle_correction = 0;
jliutang 9:d9776d8ce47c 301 lastError = 0;
jliutang 9:d9776d8ce47c 302 d_anglemax = 0;
csharer 4:2512939c10f0 303 }
csharer 6:ae3e6aefe908 304
csharer 6:ae3e6aefe908 305 /* Here are some loops that trigger at different intervals, this
csharer 6:ae3e6aefe908 306 * will allow you to do things at a slower rate, thus saving speed
csharer 6:ae3e6aefe908 307 * it is important to keep this fast so we dont miss IMU readings */
csharer 6:ae3e6aefe908 308
csharer 6:ae3e6aefe908 309 //Fast Loop: Good for printing to serial monitor
csharer 4:2512939c10f0 310 if(loop_counter >= 5) {
csharer 4:2512939c10f0 311 loop_counter = 0;
jliutang 9:d9776d8ce47c 312 //pc.printf("angle:%0.3f Kp1:%0.3f Kd1:%0.2f Kp2:%0.2f Kd2:%0.3f pos_M1:%d pos_M2:%d dt:%0.2f, %0.2f\r\n", angle, Kp1, Kd1, Kp2, Kd2, pos_M1, pos_M2, robot_pos, throttle, steering);
csharer 4:2512939c10f0 313 }
csharer 3:2f76ffbc5cef 314
csharer 6:ae3e6aefe908 315 //Meduim Loop: Good for sending and receiving
csharer 4:2512939c10f0 316 if (medium_loop_counter >= 10) {
csharer 4:2512939c10f0 317 medium_loop_counter = 0; // Read status
csharer 3:2f76ffbc5cef 318
csharer 4:2512939c10f0 319 //Recieve Data
csharer 6:ae3e6aefe908 320 rxLen = rf_receive(rxBuffer, 128);
csharer 6:ae3e6aefe908 321 if(rxLen > 0) {
csharer 6:ae3e6aefe908 322 led1 = led1^1;
csharer 6:ae3e6aefe908 323 //Process data with our protocal
csharer 6:ae3e6aefe908 324 communication_protocal(rxLen);
csharer 4:2512939c10f0 325 }
csharer 6:ae3e6aefe908 326
csharer 4:2512939c10f0 327 } // End of medium loop
csharer 6:ae3e6aefe908 328
csharer 6:ae3e6aefe908 329 //Slow Loop: Good for sending if speed is not an issue
csharer 4:2512939c10f0 330 if(slow_loop_counter >= 99) {
csharer 4:2512939c10f0 331 slow_loop_counter = 0;
jliutang 9:d9776d8ce47c 332 sprintf(txBuffer, "M1: %d, M2: %d, error: %0.2f, angle: %0.3f, dangle: %0.2f correction: %0.3f, Kp: %0.2f, Kd: %0.2f, motor1: %d", \
jliutang 9:d9776d8ce47c 333 pos_M1, pos_M2, error, angle, d_angle, d_anglemax, Kp1, Kd1, motor1);
jliutang 9:d9776d8ce47c 334 rf_send(txBuffer, strlen(txBuffer)+1);
csharer 4:2512939c10f0 335 } //End of Slow Loop
csharer 3:2f76ffbc5cef 336
csharer 4:2512939c10f0 337 //Reattach interupt
csharer 4:2512939c10f0 338 checkpin.rise(&dmpDataReady);
csharer 4:2512939c10f0 339 } //END WHILE
csharer 4:2512939c10f0 340
jliutang 9:d9776d8ce47c 341 /**** Update Values DO NOT MODIFY ********/
jliutang 9:d9776d8ce47c 342 loop_counter++;
jliutang 9:d9776d8ce47c 343 slow_loop_counter++;
jliutang 9:d9776d8ce47c 344 medium_loop_counter++;
jliutang 9:d9776d8ce47c 345 dt = (timer_value - timer_old);
jliutang 9:d9776d8ce47c 346 timer_old = timer_value;
jliutang 9:d9776d8ce47c 347 angle_old3 = angle_old2;
jliutang 9:d9776d8ce47c 348 angle_old2 = angle_old;
jliutang 9:d9776d8ce47c 349 angle_old = angle;
jliutang 9:d9776d8ce47c 350 /*****************************************/
jliutang 9:d9776d8ce47c 351
csharer 6:ae3e6aefe908 352 /********************* All IMU Handling DO NOT MODIFY *****************/
csharer 4:2512939c10f0 353 //Disable IRQ
csharer 4:2512939c10f0 354 checkpin.rise(NULL);
csharer 6:ae3e6aefe908 355
csharer 6:ae3e6aefe908 356 //reset interrupt flag and get INT_STATUS byte
csharer 3:2f76ffbc5cef 357 mpuInterrupt = false;
csharer 3:2f76ffbc5cef 358 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 359
csharer 6:ae3e6aefe908 360 //get current FIFO count
csharer 3:2f76ffbc5cef 361 fifoCount = mpu.getFIFOCount();
csharer 3:2f76ffbc5cef 362
csharer 3:2f76ffbc5cef 363 // check for overflow (this should never happen unless our code is too inefficient)
csharer 3:2f76ffbc5cef 364 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
csharer 3:2f76ffbc5cef 365 // reset so we can continue cleanly
csharer 3:2f76ffbc5cef 366 mpu.resetFIFO();
csharer 4:2512939c10f0 367 pc.printf("FIFO overflow!");
csharer 4:2512939c10f0 368
csharer 6:ae3e6aefe908 369 //otherwise, check for DMP data ready interrupt (this should happen frequently)
csharer 3:2f76ffbc5cef 370 } else if (mpuIntStatus & 0x02) {
csharer 6:ae3e6aefe908 371 //wait for correct available data length, should be a VERY short wait
csharer 4:2512939c10f0 372 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
csharer 3:2f76ffbc5cef 373
csharer 6:ae3e6aefe908 374 //read a packet from FIFO
csharer 3:2f76ffbc5cef 375 mpu.getFIFOBytes(fifoBuffer, packetSize);
csharer 3:2f76ffbc5cef 376
csharer 6:ae3e6aefe908 377 //track FIFO count here in case there is > 1 packet available
csharer 6:ae3e6aefe908 378 //(this lets us immediately read more without waiting for an interrupt)
csharer 3:2f76ffbc5cef 379 fifoCount -= packetSize;
csharer 3:2f76ffbc5cef 380
csharer 4:2512939c10f0 381 //Read new angle from IMU
csharer 4:2512939c10f0 382 new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
csharer 4:2512939c10f0 383 dAngle = new_angle - angle;
csharer 3:2f76ffbc5cef 384
csharer 4:2512939c10f0 385 //Filter out angle readings larger then MAX_ANGLE_DELTA
csharer 4:2512939c10f0 386 if( ((dAngle < 15) && (dAngle > -15)) || FILTER_DISABLE) {
csharer 4:2512939c10f0 387 angle = new_angle;
csharer 4:2512939c10f0 388 FILTER_DISABLE = false; //turn of filter disabler
csharer 6:ae3e6aefe908 389 }
csharer 6:ae3e6aefe908 390 }
csharer 6:ae3e6aefe908 391 /********************* All IMU Handling DO NOT MODIFY *****************/
csharer 3:2f76ffbc5cef 392
csharer 3:2f76ffbc5cef 393 } //end main loop
csharer 3:2f76ffbc5cef 394 } //End Main()