Carter Sharer / Mbed 2 deprecated BroBot_ESE350_Skeleton

BroBot Code for ESE350 Lab6 part 3 (Skeleton)

Dependencies:   MPU6050_V3 mbed-rtos mbed

Fork of BroBot_RTOS_ESE350 by Carter Sharer

main.cpp@4:2512939c10f0, 2016-10-18 (annotated)

Committer:
csharer
Date:
Tue Oct 18 20:44:21 2016 +0000
Revision:
4:2512939c10f0
Parent:
3:2f76ffbc5cef
Child:
6:62cdb7482b50
brobot version 3, this is using the old MRF24J40 Lib that used uint8_t

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 3:2f76ffbc5cef 4
csharer 4:2512939c10f0 5 //BroBot Begin
csharer 3:2f76ffbc5cef 6 #include "pin_assignments.h"
csharer 3:2f76ffbc5cef 7 #include "I2Cdev.h"
csharer 3:2f76ffbc5cef 8 #include "JJ_MPU6050_DMP_6Axis.h"
csharer 3:2f76ffbc5cef 9 #include "BroBot.h"
csharer 3:2f76ffbc5cef 10 #include "BroBot_IMU.h"
csharer 3:2f76ffbc5cef 11 #include "stepper_motors.h"
csharer 4:2512939c10f0 12 #include "MRF24J40.h"
csharer 4:2512939c10f0 13
csharer 4:2512939c10f0 14 //For RF Communication
csharer 4:2512939c10f0 15 #define JSTICK_H 8
csharer 4:2512939c10f0 16 #define JSTICK_V 9
csharer 4:2512939c10f0 17 #define SPACE 10
csharer 4:2512939c10f0 18 #define KNOB1 11
csharer 4:2512939c10f0 19 #define KNOB2 12
csharer 4:2512939c10f0 20 #define KNOB3 13
csharer 4:2512939c10f0 21 #define KNOB4 14
csharer 4:2512939c10f0 22 #define ANGLE 15
csharer 4:2512939c10f0 23 #define BUTTON 16
csharer 4:2512939c10f0 24 #define JSTICK_OFFSET 100
csharer 4:2512939c10f0 25 #define TX_BUFFER_LEN 18
csharer 4:2512939c10f0 26 #define TX_ANGLE_OFFSET 100
csharer 4:2512939c10f0 27 //Knobs
csharer 4:2512939c10f0 28 #define POT1 p17
csharer 4:2512939c10f0 29 #define POT2 p18
csharer 4:2512939c10f0 30 #define POT3 p16
csharer 4:2512939c10f0 31 #define POT4 p15
csharer 4:2512939c10f0 32 //JoyStick
csharer 4:2512939c10f0 33 #define POTV p19
csharer 4:2512939c10f0 34 #define POTH p20
csharer 3:2f76ffbc5cef 35
csharer 3:2f76ffbc5cef 36 //PID
csharer 3:2f76ffbc5cef 37 #define MAX_THROTTLE 580
csharer 3:2f76ffbc5cef 38 #define MAX_STEERING 150
csharer 3:2f76ffbc5cef 39 #define MAX_TARGET_ANGLE 12
csharer 3:2f76ffbc5cef 40 #define KP 0.19
csharer 3:2f76ffbc5cef 41 #define KD 28
csharer 3:2f76ffbc5cef 42 #define KP_THROTTLE 0.01 //0.07
csharer 3:2f76ffbc5cef 43 #define KI_THROTTLE 0//0.04
csharer 3:2f76ffbc5cef 44 #define ITERM_MAX_ERROR 25 // Iterm windup constants for PI control //40
csharer 3:2f76ffbc5cef 45 #define ITERM_MAX 8000 // 5000
csharer 3:2f76ffbc5cef 46
csharer 4:2512939c10f0 47 //MRF24J40
csharer 4:2512939c10f0 48 PinName mosi(SDI); //SDI
csharer 4:2512939c10f0 49 PinName miso(SDO); //SDO
csharer 4:2512939c10f0 50 PinName sck(SCK); //SCK
csharer 4:2512939c10f0 51 PinName cs(CS); //CS
csharer 4:2512939c10f0 52 PinName reset(RESET); //RESET
csharer 4:2512939c10f0 53 // RF tranceiver to link with handheld.
csharer 4:2512939c10f0 54 MRF24J40 mrf(mosi, miso, sck, cs, reset);
csharer 4:2512939c10f0 55 uint8_t txBuffer[128]= {1, 8, 0, 0xA1, 0xB2, 0xC3, 0xD4, 0x00};
csharer 4:2512939c10f0 56 uint8_t rxBuffer[128];
csharer 4:2512939c10f0 57 uint8_t rxLen;
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 4:2512939c10f0 63
csharer 3:2f76ffbc5cef 64 //PID Default control values from constant definitions
csharer 3:2f76ffbc5cef 65 float Kp = KP;
csharer 3:2f76ffbc5cef 66 float Kd = KD;
csharer 3:2f76ffbc5cef 67 float Kp_thr = KP_THROTTLE;
csharer 3:2f76ffbc5cef 68 float Ki_thr = KI_THROTTLE;
csharer 4:2512939c10f0 69 float Kd_thr; //Added for CS Pos contorl
csharer 3:2f76ffbc5cef 70 float Kp_user = KP;
csharer 3:2f76ffbc5cef 71 float Kd_user = KD;
csharer 3:2f76ffbc5cef 72 float Kp_thr_user = KP_THROTTLE;
csharer 3:2f76ffbc5cef 73 float Ki_thr_user = KI_THROTTLE;
csharer 3:2f76ffbc5cef 74 bool newControlParameters = false;
csharer 3:2f76ffbc5cef 75 bool modifing_control_parameters = false;
csharer 3:2f76ffbc5cef 76 float PID_errorSum;
csharer 3:2f76ffbc5cef 77 float PID_errorOld = 0;
csharer 3:2f76ffbc5cef 78 float PID_errorOld2 = 0;
csharer 3:2f76ffbc5cef 79 float setPointOld = 0;
csharer 3:2f76ffbc5cef 80 float target_angle;
csharer 3:2f76ffbc5cef 81 float throttle = 0;
csharer 3:2f76ffbc5cef 82 float steering = 0;
csharer 3:2f76ffbc5cef 83 float max_throttle = MAX_THROTTLE;
csharer 3:2f76ffbc5cef 84 float max_steering = MAX_STEERING;
csharer 3:2f76ffbc5cef 85 float max_target_angle = MAX_TARGET_ANGLE;
csharer 3:2f76ffbc5cef 86 float control_output;
csharer 3:2f76ffbc5cef 87 int16_t actual_robot_speed; // overall robot speed (measured from steppers speed)
csharer 3:2f76ffbc5cef 88 int16_t actual_robot_speed_old;
csharer 3:2f76ffbc5cef 89 float estimated_speed_filtered; // Estimated robot speed
csharer 4:2512939c10f0 90 int robot_pos = 0;
csharer 3:2f76ffbc5cef 91
csharer 3:2f76ffbc5cef 92 Timer timer;
csharer 3:2f76ffbc5cef 93 int timer_value; //maybe make this a long
csharer 3:2f76ffbc5cef 94 int timer_old; //maybe make this a long
csharer 4:2512939c10f0 95 int dt;
csharer 3:2f76ffbc5cef 96
csharer 3:2f76ffbc5cef 97 uint8_t slow_loop_counter;
csharer 4:2512939c10f0 98 uint8_t medium_loop_counter;
csharer 3:2f76ffbc5cef 99 uint8_t loop_counter;
csharer 3:2f76ffbc5cef 100
csharer 4:2512939c10f0 101
csharer 3:2f76ffbc5cef 102 Serial pc(USBTX, USBRX);
csharer 3:2f76ffbc5cef 103
csharer 4:2512939c10f0 104 // LEDs
csharer 4:2512939c10f0 105 DigitalOut led1(LED1);
csharer 4:2512939c10f0 106 DigitalOut led2(LED2);
csharer 4:2512939c10f0 107 DigitalOut led3(LED3);
csharer 4:2512939c10f0 108 DigitalOut led4(LED4);
csharer 4:2512939c10f0 109
csharer 4:2512939c10f0 110 //Button
csharer 4:2512939c10f0 111 bool button;
csharer 4:2512939c10f0 112
csharer 3:2f76ffbc5cef 113 // =============================================================================
csharer 3:2f76ffbc5cef 114 // === PD controller implementation(Proportional, derivative) ===
csharer 3:2f76ffbc5cef 115 // =============================================================================
csharer 3:2f76ffbc5cef 116 // PD controller implementation(Proportional, derivative). DT is in miliseconds
csharer 3:2f76ffbc5cef 117 // stabilityPDControl( dt, angle, target_angle, Kp, Kd);
csharer 3:2f76ffbc5cef 118 float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd)
csharer 3:2f76ffbc5cef 119 {
csharer 3:2f76ffbc5cef 120 float error;
csharer 3:2f76ffbc5cef 121 float output;
csharer 3:2f76ffbc5cef 122
csharer 3:2f76ffbc5cef 123 error = setPoint - input;
csharer 3:2f76ffbc5cef 124
csharer 4:2512939c10f0 125
csharer 3:2f76ffbc5cef 126 // Kd is implemented in two parts
csharer 3:2f76ffbc5cef 127 // The biggest one using only the input (sensor) part not the SetPoint input-input(t-2)
csharer 3:2f76ffbc5cef 128 // And the second using the setpoint to make it a bit more agressive setPoint-setPoint(t-1)
csharer 4:2512939c10f0 129 output = Kp * error; //+ (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;
csharer 4:2512939c10f0 130
csharer 4:2512939c10f0 131 PID_errorOld2 = PID_errorOld;
csharer 4:2512939c10f0 132 PID_errorOld = input; // error for Kd is only the input component
csharer 4:2512939c10f0 133 setPointOld = setPoint;
csharer 3:2f76ffbc5cef 134 return output;
csharer 4:2512939c10f0 135
csharer 4:2512939c10f0 136
csharer 3:2f76ffbc5cef 137 }
csharer 3:2f76ffbc5cef 138
csharer 3:2f76ffbc5cef 139 // PI controller implementation (Proportional, integral). DT is in miliseconds
csharer 3:2f76ffbc5cef 140 float speedPIControl(float DT, float input, float setPoint, float Kp, float Ki)
csharer 3:2f76ffbc5cef 141 {
csharer 3:2f76ffbc5cef 142 float error;
csharer 3:2f76ffbc5cef 143 float output;
csharer 3:2f76ffbc5cef 144
csharer 3:2f76ffbc5cef 145 error = setPoint - input;
csharer 3:2f76ffbc5cef 146 PID_errorSum += CAP(error, ITERM_MAX_ERROR);
csharer 3:2f76ffbc5cef 147 PID_errorSum = CAP(PID_errorSum, ITERM_MAX);
csharer 3:2f76ffbc5cef 148
csharer 3:2f76ffbc5cef 149 //Serial.println(PID_errorSum);
csharer 3:2f76ffbc5cef 150
csharer 3:2f76ffbc5cef 151 output = Kp * error + Ki * PID_errorSum * DT * 0.001; // DT is in miliseconds...
csharer 3:2f76ffbc5cef 152 return (output);
csharer 3:2f76ffbc5cef 153 }
csharer 3:2f76ffbc5cef 154 // ================================================================
csharer 3:2f76ffbc5cef 155 // === INITIAL SETUP ===
csharer 3:2f76ffbc5cef 156 // ================================================================
csharer 3:2f76ffbc5cef 157 void init_imu()
csharer 3:2f76ffbc5cef 158 {
csharer 3:2f76ffbc5cef 159 pc.printf("\r\r\n\n Start \r\n");
csharer 3:2f76ffbc5cef 160
csharer 3:2f76ffbc5cef 161 // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
csharer 3:2f76ffbc5cef 162 mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
csharer 3:2f76ffbc5cef 163 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
csharer 3:2f76ffbc5cef 164 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
csharer 3:2f76ffbc5cef 165 mpu.setDLPFMode(MPU6050_DLPF_BW_10); //10,20,42,98,188 // Default factor for BROBOT:10
csharer 4:2512939c10f0 166 mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
csharer 3:2f76ffbc5cef 167 mpu.setSleepEnabled(false);
csharer 3:2f76ffbc5cef 168 wait_ms(500);
csharer 3:2f76ffbc5cef 169
csharer 3:2f76ffbc5cef 170 // load and configure the DMP
csharer 3:2f76ffbc5cef 171 devStatus = mpu.dmpInitialize();
csharer 3:2f76ffbc5cef 172 if(devStatus == 0) {
csharer 3:2f76ffbc5cef 173 mpu.setDMPEnabled(true);
csharer 3:2f76ffbc5cef 174 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 175 dmpReady = true;
csharer 4:2512939c10f0 176 } else {
csharer 3:2f76ffbc5cef 177 // 1 = initial memory load failed
csharer 3:2f76ffbc5cef 178 // 2 = DMP configuration updates failed
csharer 3:2f76ffbc5cef 179 pc.printf("DMP INIT error \r\n");
csharer 3:2f76ffbc5cef 180 }
csharer 3:2f76ffbc5cef 181
csharer 3:2f76ffbc5cef 182 //Gyro Calibration
csharer 3:2f76ffbc5cef 183 wait_ms(500);
csharer 3:2f76ffbc5cef 184 pc.printf("Gyro calibration!! Dont move the robot in 10 seconds... \r\n");
csharer 3:2f76ffbc5cef 185 wait_ms(500);
csharer 4:2512939c10f0 186
csharer 3:2f76ffbc5cef 187 // verify connection
csharer 3:2f76ffbc5cef 188 pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");
csharer 3:2f76ffbc5cef 189
csharer 3:2f76ffbc5cef 190 //Adjust Sensor Fusion Gain
csharer 3:2f76ffbc5cef 191 dmpSetSensorFusionAccelGain(0x20);
csharer 3:2f76ffbc5cef 192
csharer 3:2f76ffbc5cef 193 wait_ms(200);
csharer 3:2f76ffbc5cef 194 mpu.resetFIFO();
csharer 3:2f76ffbc5cef 195 }
csharer 3:2f76ffbc5cef 196
csharer 3:2f76ffbc5cef 197 // ================================================================
csharer 3:2f76ffbc5cef 198 // === MAIN PROGRAM LOOP ===
csharer 3:2f76ffbc5cef 199 // ================================================================
csharer 4:2512939c10f0 200 //CS PID CONTROLLER TEST
csharer 4:2512939c10f0 201 float target_angle_old = 0;
csharer 4:2512939c10f0 202 float change_in_target_angle = 0;
csharer 4:2512939c10f0 203 float change_in_angle = 0;
csharer 4:2512939c10f0 204 float angle_old1 = 0;
csharer 4:2512939c10f0 205 float angle_old2 = 0;
csharer 4:2512939c10f0 206 float kp_term = 0;
csharer 4:2512939c10f0 207 float kd_term = 0;
csharer 4:2512939c10f0 208 float error;
csharer 4:2512939c10f0 209 //For Position controller
csharer 4:2512939c10f0 210 float pos_error = 0;
csharer 4:2512939c10f0 211 float kp_pos_term = 0;
csharer 4:2512939c10f0 212 float kd_pos_term = 0;
csharer 4:2512939c10f0 213 float change_in_target_pos;
csharer 4:2512939c10f0 214 float target_pos, target_pos_old;
csharer 4:2512939c10f0 215 float change_in_pos;
csharer 4:2512939c10f0 216 float robot_pos_old, robot_pos_old1, robot_pos_old2;
csharer 4:2512939c10f0 217
csharer 3:2f76ffbc5cef 218 int main()
csharer 3:2f76ffbc5cef 219 {
csharer 4:2512939c10f0 220 pc.baud(230400);
csharer 3:2f76ffbc5cef 221 pc.printf("Start\r\n");
csharer 3:2f76ffbc5cef 222 init_imu();
csharer 3:2f76ffbc5cef 223 timer.start();
csharer 3:2f76ffbc5cef 224 //timer
csharer 4:2512939c10f0 225 timer_value = timer.read_us();
syundo0730 0:8d2c753a96e7 226
csharer 3:2f76ffbc5cef 227 //Init Stepper Motors
csharer 3:2f76ffbc5cef 228 //Attach Timer Interupts (Tiker)
csharer 3:2f76ffbc5cef 229 timer_M1.attach_us(&ISR1, ZERO_SPEED);
csharer 3:2f76ffbc5cef 230 timer_M2.attach_us(&ISR2, ZERO_SPEED);
csharer 3:2f76ffbc5cef 231 step_M1 = 1;
csharer 3:2f76ffbc5cef 232 dir_M1 = 1;
csharer 3:2f76ffbc5cef 233 enable = ENABLE; //Enable Motors
csharer 3:2f76ffbc5cef 234
csharer 3:2f76ffbc5cef 235 //Set Gains
csharer 3:2f76ffbc5cef 236 Kp_thr = 0; //0.15;
csharer 3:2f76ffbc5cef 237 Ki_thr = 0; //0.15;
csharer 4:2512939c10f0 238
csharer 4:2512939c10f0 239 //Attach Interupt for IMU
csharer 4:2512939c10f0 240 checkpin.rise(&dmpDataReady);
csharer 4:2512939c10f0 241
csharer 4:2512939c10f0 242 //Used to set angle upon startup, filter
csharer 4:2512939c10f0 243 bool FILTER_DISABLE = true;
csharer 4:2512939c10f0 244
csharer 3:2f76ffbc5cef 245 while(1) {
csharer 4:2512939c10f0 246
csharer 4:2512939c10f0 247 if(button) {
csharer 4:2512939c10f0 248 pos_M1 = 0;
csharer 4:2512939c10f0 249 pos_M2 = 0;
csharer 4:2512939c10f0 250 target_pos = 0;
csharer 4:2512939c10f0 251 }
csharer 4:2512939c10f0 252
csharer 4:2512939c10f0 253 while(!mpuInterrupt) { // && fifoCount < packetSize) {
csharer 4:2512939c10f0 254 //led4 = led4^1;
csharer 4:2512939c10f0 255 //pc.printf("In while comp loop \r\n");
csharer 4:2512939c10f0 256 timer_value = timer.read_us();
csharer 4:2512939c10f0 257
csharer 4:2512939c10f0 258 //Set Gainz with knobs
csharer 4:2512939c10f0 259 Kp = ((float)knob1) / 1000.0;
csharer 4:2512939c10f0 260 Kd = ((float)knob2) / 1.0;
csharer 4:2512939c10f0 261 Kp_thr = ((float)knob3) / 1000.0;
csharer 4:2512939c10f0 262 Kd_thr = ((float)knob4) / 100.0;
csharer 4:2512939c10f0 263
csharer 4:2512939c10f0 264 //Joystick control
csharer 4:2512939c10f0 265 throttle = (float)jstick_v /10.0;
csharer 4:2512939c10f0 266 steering = (float)jstick_h / 10.0;
csharer 4:2512939c10f0 267
csharer 4:2512939c10f0 268 //Update Values
csharer 3:2f76ffbc5cef 269 loop_counter++;
csharer 3:2f76ffbc5cef 270 slow_loop_counter++;
csharer 4:2512939c10f0 271 medium_loop_counter++;
csharer 3:2f76ffbc5cef 272 dt = (timer_value - timer_old);
csharer 3:2f76ffbc5cef 273 timer_old = timer_value;
csharer 4:2512939c10f0 274 angle_old = angle;
csharer 3:2f76ffbc5cef 275
csharer 4:2512939c10f0 276 // Motor contorl
csharer 4:2512939c10f0 277 if((angle < 45) && (angle > -45)) {
csharer 3:2f76ffbc5cef 278
csharer 4:2512939c10f0 279 //PID CONTROL MAGIC GOES HERE
csharer 4:2512939c10f0 280 // We calculate the estimated robot speed:
csharer 4:2512939c10f0 281 // Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU)
csharer 4:2512939c10f0 282 actual_robot_speed_old = actual_robot_speed;
csharer 4:2512939c10f0 283 actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward
csharer 4:2512939c10f0 284 int16_t angular_velocity = (angle - angle_old) * 90.0; // 90 is an empirical extracted factor to adjust for real units
csharer 4:2512939c10f0 285 int16_t estimated_speed = -actual_robot_speed_old - angular_velocity; // We use robot_speed(t-1) or (t-2) to compensate the delay
csharer 4:2512939c10f0 286 estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05; // low pass filter on estimated speed
csharer 4:2512939c10f0 287 // SPEED CONTROL: This is a PI controller.
csharer 4:2512939c10f0 288 // input:user throttle, variable: estimated robot speed, output: target robot angle to get the desired speed
csharer 4:2512939c10f0 289 //CS target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
csharer 4:2512939c10f0 290 //CS target_angle = CAP(target_angle, max_target_angle); // limited output
csharer 4:2512939c10f0 291 //target_angle = 0;
csharer 4:2512939c10f0 292 // Stability control: This is a PD controller.
csharer 4:2512939c10f0 293 // input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed
csharer 4:2512939c10f0 294 // We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed.
csharer 3:2f76ffbc5cef 295
csharer 4:2512939c10f0 296 //pc.printf("dt: %f, angle: %f, target_angle: %f, Kp: %f, Kd: %f \r\n", dt, angle, target_angle, Kp, Kd);
csharer 4:2512939c10f0 297 //control_output = stabilityPDControl(dt, angle, target_angle, Kp, Kd);
csharer 4:2512939c10f0 298
csharer 4:2512939c10f0 299 //CS Pd Target Angle Contoller Goes Here
csharer 4:2512939c10f0 300 target_pos += throttle;
csharer 4:2512939c10f0 301 robot_pos = (pos_M1 + pos_M2) / 2;
csharer 4:2512939c10f0 302 //KP Term
csharer 4:2512939c10f0 303 pos_error = robot_pos - target_pos; //robot_pos - target_pos;
csharer 4:2512939c10f0 304 kp_pos_term = -Kp_thr * pos_error;
csharer 4:2512939c10f0 305
csharer 4:2512939c10f0 306 //KD Term
csharer 4:2512939c10f0 307 change_in_target_pos = target_pos - target_pos_old;
csharer 4:2512939c10f0 308 change_in_pos = robot_pos - robot_pos_old2;
csharer 4:2512939c10f0 309 kd_pos_term = ((-Kd_thr * change_in_target_pos) - (-Kd_thr*change_in_pos)) /dt;
csharer 4:2512939c10f0 310 target_angle = kp_pos_term + kd_pos_term;
csharer 4:2512939c10f0 311 target_angle = CAP(target_angle, MAX_TARGET_ANGLE);
csharer 4:2512939c10f0 312
csharer 4:2512939c10f0 313 //Update values
csharer 4:2512939c10f0 314 target_pos_old = target_pos;
csharer 4:2512939c10f0 315 robot_pos_old2 = robot_pos_old1;
csharer 4:2512939c10f0 316 robot_pos_old1 = robot_pos_old;
csharer 4:2512939c10f0 317
csharer 4:2512939c10f0 318 //CS PD Stability CONTROLLER HERE
csharer 4:2512939c10f0 319 error = target_angle - angle;
csharer 4:2512939c10f0 320 kp_term = Kp * error;
csharer 4:2512939c10f0 321
csharer 4:2512939c10f0 322 change_in_target_angle = target_angle - target_angle_old; //add
csharer 4:2512939c10f0 323 change_in_angle = angle - angle_old2; //add
csharer 4:2512939c10f0 324 kd_term = ((Kd * change_in_target_angle) - Kd*(change_in_angle)) / dt;
csharer 4:2512939c10f0 325
csharer 4:2512939c10f0 326 //Control Output
csharer 4:2512939c10f0 327 control_output += kp_term + kd_term;
csharer 4:2512939c10f0 328 control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
csharer 4:2512939c10f0 329 motor1 = (int16_t)(control_output + (steering/4));
csharer 4:2512939c10f0 330 motor2 = (int16_t)(control_output - (steering/4));
csharer 4:2512939c10f0 331 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
csharer 4:2512939c10f0 332 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
csharer 4:2512939c10f0 333
csharer 4:2512939c10f0 334 //Update variables
csharer 4:2512939c10f0 335 target_angle_old = target_angle;
csharer 4:2512939c10f0 336 angle_old2 = angle_old1;
csharer 4:2512939c10f0 337 angle_old1 = angle;
csharer 4:2512939c10f0 338
csharer 4:2512939c10f0 339 //Enable Motors
csharer 4:2512939c10f0 340 enable = ENABLE;
csharer 4:2512939c10f0 341 setMotor1Speed(-motor1);
csharer 4:2512939c10f0 342 setMotor2Speed(-motor2);
csharer 4:2512939c10f0 343 robot_pos += (-motor1 + -motor2) / 2;
csharer 4:2512939c10f0 344 //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 4:2512939c10f0 345 } else {
csharer 4:2512939c10f0 346 //Disable Motors
csharer 4:2512939c10f0 347 enable = DISABLE;
csharer 4:2512939c10f0 348 //Set Motor Speed 0
csharer 4:2512939c10f0 349 PID_errorSum = 0; // Reset PID I term
csharer 4:2512939c10f0 350 }
csharer 4:2512939c10f0 351
csharer 4:2512939c10f0 352 //Fast Loop
csharer 4:2512939c10f0 353 if(loop_counter >= 5) {
csharer 4:2512939c10f0 354 loop_counter = 0;
csharer 4:2512939c10f0 355 //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);
csharer 4:2512939c10f0 356 //setMotor1Speed(int16_t(angle));
csharer 4:2512939c10f0 357 //setMotor2Speed(int16_t(angle));
csharer 4:2512939c10f0 358 //pc.printf("horz: %d verti: %d knob1: %d angle: %d \r\n", jstick_h, jstick_v, knob1, (int)angle);
csharer 4:2512939c10f0 359 //pc.printf("angle: %d \r\n", (int)angle);
csharer 4:2512939c10f0 360 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);
csharer 4:2512939c10f0 361 }
csharer 3:2f76ffbc5cef 362
csharer 3:2f76ffbc5cef 363
csharer 4:2512939c10f0 364 //Meduim Loop
csharer 4:2512939c10f0 365 if (medium_loop_counter >= 10) {
csharer 4:2512939c10f0 366 medium_loop_counter = 0; // Read status
csharer 4:2512939c10f0 367 led2 = led2^1;
csharer 3:2f76ffbc5cef 368
csharer 4:2512939c10f0 369 //Recieve Data
csharer 4:2512939c10f0 370 rxLen = mrf.Receive(rxBuffer, 128);
csharer 4:2512939c10f0 371 if(rxLen) {
csharer 4:2512939c10f0 372 if((rxBuffer[0] == (uint8_t)1) && (rxBuffer[1] == (uint8_t)8) && (rxBuffer[2]==(uint8_t)0)) {
csharer 4:2512939c10f0 373 jstick_h = (int8_t)rxBuffer[JSTICK_H] - JSTICK_OFFSET;
csharer 4:2512939c10f0 374 jstick_v = (int8_t)rxBuffer[JSTICK_V] - JSTICK_OFFSET;
csharer 4:2512939c10f0 375 knob1 = rxBuffer[KNOB1];
csharer 4:2512939c10f0 376 knob2 = rxBuffer[KNOB2];
csharer 4:2512939c10f0 377 knob3 = rxBuffer[KNOB3];
csharer 4:2512939c10f0 378 knob4 = rxBuffer[KNOB4];
csharer 4:2512939c10f0 379 button = rxBuffer[BUTTON];
csharer 4:2512939c10f0 380 led1= led1^1; //flash led for debuggin
csharer 4:2512939c10f0 381 led4 = button;
csharer 4:2512939c10f0 382 }
csharer 4:2512939c10f0 383 } else {
csharer 4:2512939c10f0 384 mrf.Reset();
csharer 4:2512939c10f0 385 }
csharer 4:2512939c10f0 386 } // End of medium loop
csharer 4:2512939c10f0 387
csharer 4:2512939c10f0 388 //Slow Loop
csharer 4:2512939c10f0 389 if(slow_loop_counter >= 99) {
csharer 4:2512939c10f0 390 slow_loop_counter = 0;
csharer 3:2f76ffbc5cef 391
csharer 4:2512939c10f0 392 //Send Data
csharer 4:2512939c10f0 393 txBuffer[ANGLE] = (uint8_t)(angle + TX_ANGLE_OFFSET);
csharer 4:2512939c10f0 394 mrf.Send(txBuffer, TX_BUFFER_LEN);
csharer 4:2512939c10f0 395 } //End of Slow Loop
csharer 3:2f76ffbc5cef 396
csharer 4:2512939c10f0 397 //Reattach interupt
csharer 4:2512939c10f0 398 checkpin.rise(&dmpDataReady);
csharer 4:2512939c10f0 399 } //END WHILE
csharer 4:2512939c10f0 400
csharer 4:2512939c10f0 401 //Disable IRQ
csharer 4:2512939c10f0 402 checkpin.rise(NULL);
csharer 4:2512939c10f0 403 led3 = led3^1;
csharer 4:2512939c10f0 404 //pc.printf("taking care of imu stuff angle: %f \r\n", angle);
csharer 4:2512939c10f0 405 //All IMU stuff
csharer 3:2f76ffbc5cef 406 // reset interrupt flag and get INT_STATUS byte
csharer 3:2f76ffbc5cef 407 mpuInterrupt = false;
csharer 3:2f76ffbc5cef 408 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 409
csharer 3:2f76ffbc5cef 410 // get current FIFO count
csharer 3:2f76ffbc5cef 411 fifoCount = mpu.getFIFOCount();
csharer 3:2f76ffbc5cef 412
csharer 3:2f76ffbc5cef 413 // check for overflow (this should never happen unless our code is too inefficient)
csharer 3:2f76ffbc5cef 414 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
csharer 3:2f76ffbc5cef 415 // reset so we can continue cleanly
csharer 3:2f76ffbc5cef 416 mpu.resetFIFO();
csharer 4:2512939c10f0 417 pc.printf("FIFO overflow!");
csharer 4:2512939c10f0 418
csharer 3:2f76ffbc5cef 419 // otherwise, check for DMP data ready interrupt (this should happen frequently)
csharer 3:2f76ffbc5cef 420 } else if (mpuIntStatus & 0x02) {
csharer 3:2f76ffbc5cef 421 // wait for correct available data length, should be a VERY short wait
csharer 4:2512939c10f0 422 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
csharer 3:2f76ffbc5cef 423
csharer 3:2f76ffbc5cef 424 // read a packet from FIFO
csharer 3:2f76ffbc5cef 425 mpu.getFIFOBytes(fifoBuffer, packetSize);
csharer 3:2f76ffbc5cef 426
csharer 3:2f76ffbc5cef 427 // track FIFO count here in case there is > 1 packet available
csharer 3:2f76ffbc5cef 428 // (this lets us immediately read more without waiting for an interrupt)
csharer 3:2f76ffbc5cef 429 fifoCount -= packetSize;
csharer 3:2f76ffbc5cef 430
csharer 4:2512939c10f0 431 //Read new angle from IMU
csharer 4:2512939c10f0 432 new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
csharer 4:2512939c10f0 433 dAngle = new_angle - angle;
csharer 3:2f76ffbc5cef 434
csharer 3:2f76ffbc5cef 435
csharer 4:2512939c10f0 436 //Filter out angle readings larger then MAX_ANGLE_DELTA
csharer 4:2512939c10f0 437 if( ((dAngle < 15) && (dAngle > -15)) || FILTER_DISABLE) {
csharer 4:2512939c10f0 438 angle = new_angle;
csharer 4:2512939c10f0 439 FILTER_DISABLE = false; //turn of filter disabler
csharer 4:2512939c10f0 440 } else {
csharer 4:2512939c10f0 441 pc.printf("\t\t\t filtered angle \r\n");
csharer 4:2512939c10f0 442 }
csharer 4:2512939c10f0 443 //END IMU STUFF
csharer 3:2f76ffbc5cef 444
csharer 3:2f76ffbc5cef 445 }
csharer 3:2f76ffbc5cef 446 } //end main loop
csharer 3:2f76ffbc5cef 447 } //End Main()