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@9:fb671fa0c0be, 2017-01-09 (annotated)

Committer:
csharer
Date:
Mon Jan 09 17:50:35 2017 +0000
Revision:
9:fb671fa0c0be
Parent:
8:8389c0a9339e
Child:
10:48f640a54401
mbed rtos

Who changed what in which revision?

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