Carter Sharer / Mbed 2 deprecated ESE519_Lab6_part3_skeleton

ESE519 Lab6 Part3

Dependencies:   MPU6050_Lab6_Part3 mbed

Fork of BroBot_v2 by Carter Sharer

main.cpp@3:2f76ffbc5cef, 2016-10-12 (annotated)

Committer:
csharer
Date:
Wed Oct 12 05:04:10 2016 +0000
Revision:
3:2f76ffbc5cef
Parent:
2:42c4f3a7813f
Child:
4:2512939c10f0
first commit to BroBot;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
csharer 3:2f76ffbc5cef 1 //BroBot V2
csharer 3:2f76ffbc5cef 2 //Author: Carter Sharer
csharer 3:2f76ffbc5cef 3 //Date: 10/11/2016
csharer 3:2f76ffbc5cef 4
csharer 3:2f76ffbc5cef 5 #include "pin_assignments.h"
csharer 3:2f76ffbc5cef 6 #include "I2Cdev.h"
csharer 3:2f76ffbc5cef 7 #include "JJ_MPU6050_DMP_6Axis.h"
csharer 3:2f76ffbc5cef 8 #include "BroBot.h"
csharer 3:2f76ffbc5cef 9 #include "BroBot_IMU.h"
csharer 3:2f76ffbc5cef 10 #include "stepper_motors.h"
csharer 3:2f76ffbc5cef 11
csharer 3:2f76ffbc5cef 12 //PID
csharer 3:2f76ffbc5cef 13 #define MAX_THROTTLE 580
csharer 3:2f76ffbc5cef 14 #define MAX_STEERING 150
csharer 3:2f76ffbc5cef 15 #define MAX_TARGET_ANGLE 12
csharer 3:2f76ffbc5cef 16 #define KP 0.19
csharer 3:2f76ffbc5cef 17 #define KD 28
csharer 3:2f76ffbc5cef 18 #define KP_THROTTLE 0.01 //0.07
csharer 3:2f76ffbc5cef 19 #define KI_THROTTLE 0//0.04
csharer 3:2f76ffbc5cef 20 #define ITERM_MAX_ERROR 25 // Iterm windup constants for PI control //40
csharer 3:2f76ffbc5cef 21 #define ITERM_MAX 8000 // 5000
csharer 3:2f76ffbc5cef 22
csharer 3:2f76ffbc5cef 23 //PID Default control values from constant definitions
csharer 3:2f76ffbc5cef 24 float Kp = KP;
csharer 3:2f76ffbc5cef 25 float Kd = KD;
csharer 3:2f76ffbc5cef 26 float Kp_thr = KP_THROTTLE;
csharer 3:2f76ffbc5cef 27 float Ki_thr = KI_THROTTLE;
csharer 3:2f76ffbc5cef 28 float Kp_user = KP;
csharer 3:2f76ffbc5cef 29 float Kd_user = KD;
csharer 3:2f76ffbc5cef 30 float Kp_thr_user = KP_THROTTLE;
csharer 3:2f76ffbc5cef 31 float Ki_thr_user = KI_THROTTLE;
csharer 3:2f76ffbc5cef 32 bool newControlParameters = false;
csharer 3:2f76ffbc5cef 33 bool modifing_control_parameters = false;
csharer 3:2f76ffbc5cef 34 float PID_errorSum;
csharer 3:2f76ffbc5cef 35 float PID_errorOld = 0;
csharer 3:2f76ffbc5cef 36 float PID_errorOld2 = 0;
csharer 3:2f76ffbc5cef 37 float setPointOld = 0;
csharer 3:2f76ffbc5cef 38 float target_angle;
csharer 3:2f76ffbc5cef 39 float throttle = 0;
csharer 3:2f76ffbc5cef 40 float steering = 0;
csharer 3:2f76ffbc5cef 41 float max_throttle = MAX_THROTTLE;
csharer 3:2f76ffbc5cef 42 float max_steering = MAX_STEERING;
csharer 3:2f76ffbc5cef 43 float max_target_angle = MAX_TARGET_ANGLE;
csharer 3:2f76ffbc5cef 44 float control_output;
csharer 3:2f76ffbc5cef 45 int16_t actual_robot_speed; // overall robot speed (measured from steppers speed)
csharer 3:2f76ffbc5cef 46 int16_t actual_robot_speed_old;
csharer 3:2f76ffbc5cef 47 float estimated_speed_filtered; // Estimated robot speed
csharer 3:2f76ffbc5cef 48
csharer 3:2f76ffbc5cef 49 Timer timer;
csharer 3:2f76ffbc5cef 50 int timer_value; //maybe make this a long
csharer 3:2f76ffbc5cef 51 int timer_old; //maybe make this a long
csharer 3:2f76ffbc5cef 52 float dt;
csharer 3:2f76ffbc5cef 53
csharer 3:2f76ffbc5cef 54 uint8_t slow_loop_counter;
csharer 3:2f76ffbc5cef 55 uint8_t loop_counter;
csharer 3:2f76ffbc5cef 56
csharer 3:2f76ffbc5cef 57 Serial pc(USBTX, USBRX);
csharer 3:2f76ffbc5cef 58
csharer 3:2f76ffbc5cef 59 // =============================================================================
csharer 3:2f76ffbc5cef 60 // === PD controller implementation(Proportional, derivative) ===
csharer 3:2f76ffbc5cef 61 // =============================================================================
csharer 3:2f76ffbc5cef 62 // PD controller implementation(Proportional, derivative). DT is in miliseconds
csharer 3:2f76ffbc5cef 63 // stabilityPDControl( dt, angle, target_angle, Kp, Kd);
csharer 3:2f76ffbc5cef 64 float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd)
csharer 3:2f76ffbc5cef 65 {
csharer 3:2f76ffbc5cef 66 float error;
csharer 3:2f76ffbc5cef 67 float output;
csharer 3:2f76ffbc5cef 68
csharer 3:2f76ffbc5cef 69 error = setPoint - input;
csharer 3:2f76ffbc5cef 70
csharer 3:2f76ffbc5cef 71 // Kd is implemented in two parts
csharer 3:2f76ffbc5cef 72 // The biggest one using only the input (sensor) part not the SetPoint input-input(t-2)
csharer 3:2f76ffbc5cef 73 // And the second using the setpoint to make it a bit more agressive setPoint-setPoint(t-1)
csharer 3:2f76ffbc5cef 74 output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;
csharer 3:2f76ffbc5cef 75 //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
csharer 3:2f76ffbc5cef 76 //PID_errorOld2 = PID_errorOld;
csharer 3:2f76ffbc5cef 77 //PID_errorOld = input; // error for Kd is only the input component
csharer 3:2f76ffbc5cef 78 //setPointOld = setPoint;
csharer 3:2f76ffbc5cef 79 return output;
csharer 3:2f76ffbc5cef 80 }
csharer 3:2f76ffbc5cef 81
csharer 3:2f76ffbc5cef 82 // PI controller implementation (Proportional, integral). DT is in miliseconds
csharer 3:2f76ffbc5cef 83 float speedPIControl(float DT, float input, float setPoint, float Kp, float Ki)
csharer 3:2f76ffbc5cef 84 {
csharer 3:2f76ffbc5cef 85 float error;
csharer 3:2f76ffbc5cef 86 float output;
csharer 3:2f76ffbc5cef 87
csharer 3:2f76ffbc5cef 88 error = setPoint - input;
csharer 3:2f76ffbc5cef 89 PID_errorSum += CAP(error, ITERM_MAX_ERROR);
csharer 3:2f76ffbc5cef 90 PID_errorSum = CAP(PID_errorSum, ITERM_MAX);
csharer 3:2f76ffbc5cef 91
csharer 3:2f76ffbc5cef 92 //Serial.println(PID_errorSum);
csharer 3:2f76ffbc5cef 93
csharer 3:2f76ffbc5cef 94 output = Kp * error + Ki * PID_errorSum * DT * 0.001; // DT is in miliseconds...
csharer 3:2f76ffbc5cef 95 return (output);
csharer 3:2f76ffbc5cef 96 }
csharer 3:2f76ffbc5cef 97 // ================================================================
csharer 3:2f76ffbc5cef 98 // === INITIAL SETUP ===
csharer 3:2f76ffbc5cef 99 // ================================================================
csharer 3:2f76ffbc5cef 100 void init_imu()
csharer 3:2f76ffbc5cef 101 {
csharer 3:2f76ffbc5cef 102 pc.printf("\r\r\n\n Start \r\n");
csharer 3:2f76ffbc5cef 103
csharer 3:2f76ffbc5cef 104 // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
csharer 3:2f76ffbc5cef 105 mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
csharer 3:2f76ffbc5cef 106 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
csharer 3:2f76ffbc5cef 107 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
csharer 3:2f76ffbc5cef 108 mpu.setDLPFMode(MPU6050_DLPF_BW_10); //10,20,42,98,188 // Default factor for BROBOT:10
csharer 3:2f76ffbc5cef 109 mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz 4=200hz
csharer 3:2f76ffbc5cef 110 mpu.setSleepEnabled(false);
csharer 3:2f76ffbc5cef 111 wait_ms(500);
csharer 3:2f76ffbc5cef 112
csharer 3:2f76ffbc5cef 113 // load and configure the DMP
csharer 3:2f76ffbc5cef 114 devStatus = mpu.dmpInitialize();
csharer 3:2f76ffbc5cef 115 if(devStatus == 0) {
csharer 3:2f76ffbc5cef 116 mpu.setDMPEnabled(true);
csharer 3:2f76ffbc5cef 117 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 118 dmpReady = true;
csharer 3:2f76ffbc5cef 119 } else {
csharer 3:2f76ffbc5cef 120 // 1 = initial memory load failed
csharer 3:2f76ffbc5cef 121 // 2 = DMP configuration updates failed
csharer 3:2f76ffbc5cef 122 pc.printf("DMP INIT error \r\n");
csharer 3:2f76ffbc5cef 123 }
csharer 3:2f76ffbc5cef 124
csharer 3:2f76ffbc5cef 125 //Gyro Calibration
csharer 3:2f76ffbc5cef 126 wait_ms(500);
csharer 3:2f76ffbc5cef 127 pc.printf("Gyro calibration!! Dont move the robot in 10 seconds... \r\n");
csharer 3:2f76ffbc5cef 128 wait_ms(500);
csharer 3:2f76ffbc5cef 129
csharer 3:2f76ffbc5cef 130 // verify connection
csharer 3:2f76ffbc5cef 131 pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");
csharer 3:2f76ffbc5cef 132
csharer 3:2f76ffbc5cef 133 //Adjust Sensor Fusion Gain
csharer 3:2f76ffbc5cef 134 dmpSetSensorFusionAccelGain(0x20);
csharer 3:2f76ffbc5cef 135
csharer 3:2f76ffbc5cef 136 wait_ms(200);
csharer 3:2f76ffbc5cef 137 mpu.resetFIFO();
csharer 3:2f76ffbc5cef 138 }
csharer 3:2f76ffbc5cef 139
csharer 3:2f76ffbc5cef 140 // ================================================================
csharer 3:2f76ffbc5cef 141 // === MAIN PROGRAM LOOP ===
csharer 3:2f76ffbc5cef 142 // ================================================================
csharer 3:2f76ffbc5cef 143 int main()
csharer 3:2f76ffbc5cef 144 {
csharer 3:2f76ffbc5cef 145 pc.baud(115200);
csharer 3:2f76ffbc5cef 146 pc.printf("Start\r\n");
csharer 3:2f76ffbc5cef 147 init_imu();
csharer 3:2f76ffbc5cef 148 timer.start();
csharer 3:2f76ffbc5cef 149 //timer
csharer 3:2f76ffbc5cef 150 timer_value = timer.read_ms();
syundo0730 0:8d2c753a96e7 151
csharer 3:2f76ffbc5cef 152 //Init Stepper Motors
csharer 3:2f76ffbc5cef 153 //Attach Timer Interupts (Tiker)
csharer 3:2f76ffbc5cef 154 timer_M1.attach_us(&ISR1, ZERO_SPEED);
csharer 3:2f76ffbc5cef 155 timer_M2.attach_us(&ISR2, ZERO_SPEED);
csharer 3:2f76ffbc5cef 156 step_M1 = 1;
csharer 3:2f76ffbc5cef 157 dir_M1 = 1;
csharer 3:2f76ffbc5cef 158 enable = ENABLE; //Enable Motors
csharer 3:2f76ffbc5cef 159
csharer 3:2f76ffbc5cef 160 //Set Gains
csharer 3:2f76ffbc5cef 161 Kp_thr = 0; //0.15;
csharer 3:2f76ffbc5cef 162 Ki_thr = 0; //0.15;
csharer 3:2f76ffbc5cef 163
csharer 3:2f76ffbc5cef 164 while(1) {
csharer 3:2f76ffbc5cef 165 // New DMP Orientation solution?
csharer 3:2f76ffbc5cef 166 fifoCount = mpu.getFIFOCount();
csharer 3:2f76ffbc5cef 167 if (fifoCount >= 18) {
csharer 3:2f76ffbc5cef 168 if (fifoCount > 18) { // If we have more than one packet we take the easy path: discard the buffer and wait for the next one
csharer 3:2f76ffbc5cef 169 pc.printf("FIFO RESET!!\r\n");
csharer 3:2f76ffbc5cef 170 mpu.resetFIFO();
csharer 3:2f76ffbc5cef 171 return;
csharer 3:2f76ffbc5cef 172 }
csharer 3:2f76ffbc5cef 173 loop_counter++;
csharer 3:2f76ffbc5cef 174 slow_loop_counter++;
csharer 3:2f76ffbc5cef 175 dt = (timer_value - timer_old);
csharer 3:2f76ffbc5cef 176 timer_old = timer_value;
csharer 3:2f76ffbc5cef 177
csharer 3:2f76ffbc5cef 178 angle_old = angle;
csharer 3:2f76ffbc5cef 179 // Get new orientation angle from IMU (MPU6050)
csharer 3:2f76ffbc5cef 180 angle = dmpGetPhi();
csharer 3:2f76ffbc5cef 181
csharer 3:2f76ffbc5cef 182 mpu.resetFIFO(); // We always reset FIFO
csharer 3:2f76ffbc5cef 183 } // End of new IMU data
csharer 3:2f76ffbc5cef 184
csharer 3:2f76ffbc5cef 185 if(loop_counter >= 5) {
csharer 3:2f76ffbc5cef 186 loop_counter = 0;
csharer 3:2f76ffbc5cef 187 int16_t offset =
csharer 3:2f76ffbc5cef 188 pc.printf("angle: %d \r\n", int16_t(angle-ANGLE_OFFSET));
csharer 3:2f76ffbc5cef 189 setMotor1Speed(int16_t(angle-ANGLE_OFFSET));
csharer 3:2f76ffbc5cef 190 setMotor2Speed(int16_t(angle-ANGLE_OFFSET));
csharer 3:2f76ffbc5cef 191 }
csharer 3:2f76ffbc5cef 192 if (slow_loop_counter >= 99) { // 2Hz
csharer 3:2f76ffbc5cef 193 slow_loop_counter = 0; // Read status
csharer 3:2f76ffbc5cef 194 } // End of slow loop
csharer 3:2f76ffbc5cef 195
csharer 3:2f76ffbc5cef 196
csharer 3:2f76ffbc5cef 197 /*
csharer 3:2f76ffbc5cef 198 //Set Gains
csharer 3:2f76ffbc5cef 199 Kp = 0.02;
csharer 3:2f76ffbc5cef 200 Kd = 0.01;
csharer 3:2f76ffbc5cef 201
csharer 3:2f76ffbc5cef 202 timer_value = timer.read_ms();
csharer 3:2f76ffbc5cef 203
csharer 3:2f76ffbc5cef 204 // if programming failed, don't try to do anything
csharer 3:2f76ffbc5cef 205 if (!dmpReady) return;
csharer 3:2f76ffbc5cef 206
csharer 3:2f76ffbc5cef 207 // reset interrupt flag and get INT_STATUS byte
csharer 3:2f76ffbc5cef 208 mpuInterrupt = false;
csharer 3:2f76ffbc5cef 209 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 210
csharer 3:2f76ffbc5cef 211 // get current FIFO count
csharer 3:2f76ffbc5cef 212 fifoCount = mpu.getFIFOCount();
csharer 3:2f76ffbc5cef 213
csharer 3:2f76ffbc5cef 214 // check for overflow (this should never happen unless our code is too inefficient)
csharer 3:2f76ffbc5cef 215 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
csharer 3:2f76ffbc5cef 216 // reset so we can continue cleanly
csharer 3:2f76ffbc5cef 217 mpu.resetFIFO();
csharer 3:2f76ffbc5cef 218 // otherwise, check for DMP data ready interrupt (this should happen frequently)
csharer 3:2f76ffbc5cef 219 } else if (mpuIntStatus & 0x02) {
csharer 3:2f76ffbc5cef 220 // wait for correct available data length, should be a VERY short wait
csharer 3:2f76ffbc5cef 221 //CS while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
csharer 3:2f76ffbc5cef 222
csharer 3:2f76ffbc5cef 223 // read a packet from FIFO
csharer 3:2f76ffbc5cef 224 mpu.getFIFOBytes(fifoBuffer, packetSize);
csharer 3:2f76ffbc5cef 225
csharer 3:2f76ffbc5cef 226 angle_old = angle; //Update old angle before reading new angle
syundo0730 0:8d2c753a96e7 227
csharer 3:2f76ffbc5cef 228 // track FIFO count here in case there is > 1 packet available
csharer 3:2f76ffbc5cef 229 // (this lets us immediately read more without waiting for an interrupt)
csharer 3:2f76ffbc5cef 230 fifoCount -= packetSize;
csharer 3:2f76ffbc5cef 231 mpu.dmpGetQuaternion(&q, fifoBuffer);
csharer 3:2f76ffbc5cef 232 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;
csharer 3:2f76ffbc5cef 233 angle = angle - ANGLE_OFFSET;
csharer 3:2f76ffbc5cef 234 //pc.printf("angle: %f \r\n", angle);
csharer 3:2f76ffbc5cef 235
csharer 3:2f76ffbc5cef 236
csharer 3:2f76ffbc5cef 237 //Update timer
csharer 3:2f76ffbc5cef 238 dt = (timer_value - timer_old);
csharer 3:2f76ffbc5cef 239 timer_old = timer_value;
csharer 3:2f76ffbc5cef 240
csharer 3:2f76ffbc5cef 241 //PID CONTROL MAGIC GOES HERE
csharer 3:2f76ffbc5cef 242 // We calculate the estimated robot speed:
csharer 3:2f76ffbc5cef 243 // Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU)
csharer 3:2f76ffbc5cef 244 //CS actual_robot_speed_old = actual_robot_speed;
csharer 3:2f76ffbc5cef 245 //CS actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward
csharer 3:2f76ffbc5cef 246 //CS int16_t angular_velocity = (angle - angle_old) * 90.0; // 90 is an empirical extracted factor to adjust for real units
csharer 3:2f76ffbc5cef 247 //CS int16_t estimated_speed = -actual_robot_speed_old - angular_velocity; // We use robot_speed(t-1) or (t-2) to compensate the delay
csharer 3:2f76ffbc5cef 248 //CS estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05; // low pass filter on estimated speed
csharer 3:2f76ffbc5cef 249 // SPEED CONTROL: This is a PI controller.
csharer 3:2f76ffbc5cef 250 // input:user throttle, variable: estimated robot speed, output: target robot angle to get the desired speed
csharer 3:2f76ffbc5cef 251 //CS target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
csharer 3:2f76ffbc5cef 252 //CD target_angle = constrain(target_angle, -max_target_angle, max_target_angle); // limited output
csharer 3:2f76ffbc5cef 253 // Stability control: This is a PD controller.
csharer 3:2f76ffbc5cef 254 // input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed
csharer 3:2f76ffbc5cef 255 // We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed.
csharer 3:2f76ffbc5cef 256
csharer 3:2f76ffbc5cef 257 //pc.printf("dt: %f, angle: %f, target_angle: %f, Kp: %f, Kd: %f \r\n", dt, angle, target_angle, Kp, Kd);
csharer 3:2f76ffbc5cef 258 control_output += stabilityPDControl(dt, angle, target_angle, Kp, Kd);
csharer 3:2f76ffbc5cef 259 control_output = constrain(control_output, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); // Limit max output from control
csharer 3:2f76ffbc5cef 260 motor1 = control_output + steering;
csharer 3:2f76ffbc5cef 261 motor2 = control_output - steering;
csharer 3:2f76ffbc5cef 262
csharer 3:2f76ffbc5cef 263
csharer 3:2f76ffbc5cef 264 //TEST P CONTROL
csharer 3:2f76ffbc5cef 265 float gain = 1;
csharer 3:2f76ffbc5cef 266 motor1 = angle * gain;
csharer 3:2f76ffbc5cef 267 motor2 = angle * gain;
csharer 3:2f76ffbc5cef 268 pc.printf("motor: %d control output: %f \r\n", motor1, control_output);
csharer 3:2f76ffbc5cef 269
csharer 3:2f76ffbc5cef 270 // Limit max speed (control output)
csharer 3:2f76ffbc5cef 271 motor1 = constrain(motor1, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 272 motor2 = constrain(motor2, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
csharer 3:2f76ffbc5cef 273
csharer 3:2f76ffbc5cef 274
csharer 3:2f76ffbc5cef 275 }
csharer 3:2f76ffbc5cef 276
csharer 3:2f76ffbc5cef 277
csharer 3:2f76ffbc5cef 278 // Put all the pid loop stuff here
csharer 3:2f76ffbc5cef 279 if((angle < 45) && (angle > -45)) {
csharer 3:2f76ffbc5cef 280 //Enable Motors
csharer 3:2f76ffbc5cef 281 if(motor1 == 0) {
csharer 3:2f76ffbc5cef 282 enable = DISABLE;
csharer 3:2f76ffbc5cef 283 setMotor1Speed(0);
csharer 3:2f76ffbc5cef 284 setMotor2Speed(0);
csharer 3:2f76ffbc5cef 285 } else {
csharer 3:2f76ffbc5cef 286 enable = ENABLE;
csharer 3:2f76ffbc5cef 287 setMotor1Speed(motor1);
csharer 3:2f76ffbc5cef 288 setMotor2Speed(motor2);
csharer 3:2f76ffbc5cef 289 }
csharer 3:2f76ffbc5cef 290 //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 3:2f76ffbc5cef 291 } else {
csharer 3:2f76ffbc5cef 292 //Disable Motors
csharer 3:2f76ffbc5cef 293 enable = DISABLE;
csharer 3:2f76ffbc5cef 294 //Set Motor Speed 0
csharer 3:2f76ffbc5cef 295 PID_errorSum = 0; // Reset PID I term
csharer 3:2f76ffbc5cef 296 Kp = 0;
csharer 3:2f76ffbc5cef 297 Kd = 0;
csharer 3:2f76ffbc5cef 298 Kp_thr = 0;
csharer 3:2f76ffbc5cef 299 Ki_thr = 0;
csharer 3:2f76ffbc5cef 300 }
csharer 3:2f76ffbc5cef 301
csharer 3:2f76ffbc5cef 302 *////////////
csharer 3:2f76ffbc5cef 303 } //end main loop
csharer 3:2f76ffbc5cef 304 } //End Main()