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@17:8e2824f64b91, 2017-03-21 (annotated)

Committer:
arvindnr89
Date:
Tue Mar 21 19:46:44 2017 +0000
Revision:
17:8e2824f64b91
Parent:
16:3a85662fb740
Child:
18:ee252b8f7430
Smoothened out the working of the RTOS and corrected the PID control to execute only once per run

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arvindnr89 17:8e2824f64b91 1 /*
arvindnr89 17:8e2824f64b91 2 BroBOT Running the mBed RTOS
arvindnr89 17:8e2824f64b91 3 Authors: Arvind Ramesh and Carter Sharer
arvindnr89 17:8e2824f64b91 4 */
arvindnr89 17:8e2824f64b91 5
csharer 6:62cdb7482b50 6 //Added communication protocol v1 (no type selection)
csharer 9:fb671fa0c0be 7 //Knob1 39.898 Knob2 44.381 Knob3 10.55 Knob4 18.67
csharer 9:fb671fa0c0be 8 //Knob1 13.418 Knob2 28.848 Knob3 9.45 Knob4 42.29 Good
csharer 9:fb671fa0c0be 9 //Knob1 15.373 Knob2 28.261 Knob3 10.42 Knob4 40.97 Smoother
csharer 9:fb671fa0c0be 10
csharer 9:fb671fa0c0be 11 /******************************* README USAGE *******************************
csharer 9:fb671fa0c0be 12 * This robot must be powered on while it is laying down flat on a still table
csharer 9:fb671fa0c0be 13 * This allows the robot to calibrate the IMU (~5 seconds)
csharer 9:fb671fa0c0be 14 * The motors are DISABLED when the robot tilts more then +-45 degrees from
csharer 9:fb671fa0c0be 15 * vertical. To ENABLE the motors you must lift the robot to < +- 45 degres and
csharer 9:fb671fa0c0be 16 * press the joystick button.
csharer 9:fb671fa0c0be 17 * To reset the motor positions you must press the josystick button anytime.
csharer 9:fb671fa0c0be 18 ******************************************************************************/
csharer 3:2f76ffbc5cef 19
arvindnr89 10:48f640a54401 20 //Controller Values
arvindnr89 10:48f640a54401 21 float knob1, knob2, knob3, knob4;
arvindnr89 10:48f640a54401 22 float jstick_h, jstick_v;
arvindnr89 10:48f640a54401 23
arvindnr89 10:48f640a54401 24 //Button
arvindnr89 10:48f640a54401 25 bool button;
arvindnr89 10:48f640a54401 26
csharer 4:2512939c10f0 27 //BroBot Begin
csharer 9:fb671fa0c0be 28 #include "cmsis_os.h"
arvindnr89 11:2553f5798f84 29 #include "rtos_definations.h"
csharer 3:2f76ffbc5cef 30 #include "pin_assignments.h"
csharer 3:2f76ffbc5cef 31 #include "I2Cdev.h"
csharer 3:2f76ffbc5cef 32 #include "JJ_MPU6050_DMP_6Axis.h"
csharer 3:2f76ffbc5cef 33 #include "BroBot.h"
csharer 3:2f76ffbc5cef 34 #include "BroBot_IMU.h"
csharer 3:2f76ffbc5cef 35 #include "stepper_motors.h"
csharer 4:2512939c10f0 36 #include "MRF24J40.h"
arvindnr89 10:48f640a54401 37 #include "communication.h"
arvindnr89 16:3a85662fb740 38 #include "waypoint.h"
csharer 4:2512939c10f0 39
csharer 9:fb671fa0c0be 40 //Angle Offset is used to set the natural balance point of your robot.
csharer 6:62cdb7482b50 41 //You should adjust this offset so that your robots balance points is near 0
csharer 6:62cdb7482b50 42 #define ANGLE_OFFSET 107
arvindnr89 16:3a85662fb740 43 #define THETA_OFFSET 0//165
arvindnr89 10:48f640a54401 44 #define MRF_CHANNEL 2
csharer 6:62cdb7482b50 45
csharer 4:2512939c10f0 46 //For RF Communication
csharer 4:2512939c10f0 47 #define JSTICK_H 8
csharer 4:2512939c10f0 48 #define JSTICK_V 9
csharer 4:2512939c10f0 49 #define SPACE 10
csharer 4:2512939c10f0 50 #define KNOB1 11
csharer 4:2512939c10f0 51 #define KNOB2 12
csharer 4:2512939c10f0 52 #define KNOB3 13
csharer 4:2512939c10f0 53 #define KNOB4 14
csharer 4:2512939c10f0 54 #define ANGLE 15
csharer 4:2512939c10f0 55 #define BUTTON 16
csharer 4:2512939c10f0 56 #define JSTICK_OFFSET 100
csharer 4:2512939c10f0 57 #define TX_BUFFER_LEN 18
csharer 4:2512939c10f0 58 #define TX_ANGLE_OFFSET 100
csharer 4:2512939c10f0 59 //Knobs
csharer 4:2512939c10f0 60 #define POT1 p17
csharer 4:2512939c10f0 61 #define POT2 p18
csharer 4:2512939c10f0 62 #define POT3 p16
csharer 4:2512939c10f0 63 #define POT4 p15
csharer 4:2512939c10f0 64 //JoyStick
csharer 4:2512939c10f0 65 #define POTV p19
csharer 4:2512939c10f0 66 #define POTH p20
csharer 3:2f76ffbc5cef 67
csharer 3:2f76ffbc5cef 68 //PID
csharer 3:2f76ffbc5cef 69 #define MAX_THROTTLE 580
csharer 3:2f76ffbc5cef 70 #define MAX_STEERING 150
csharer 3:2f76ffbc5cef 71 #define MAX_TARGET_ANGLE 12
csharer 3:2f76ffbc5cef 72 #define KP 0.19
csharer 3:2f76ffbc5cef 73 #define KD 28
csharer 3:2f76ffbc5cef 74 #define KP_THROTTLE 0.01 //0.07
csharer 3:2f76ffbc5cef 75 #define KI_THROTTLE 0//0.04
csharer 3:2f76ffbc5cef 76 #define ITERM_MAX_ERROR 25 // Iterm windup constants for PI control //40
csharer 3:2f76ffbc5cef 77 #define ITERM_MAX 8000 // 5000
csharer 3:2f76ffbc5cef 78
arvindnr89 10:48f640a54401 79
csharer 14:94c65d1c8bad 80 #define THROTTLE_DAMPNER 100
arvindnr89 16:3a85662fb740 81 #define STREEING_DAMPNER 10000
arvindnr89 17:8e2824f64b91 82 #define VELOCITY_SAMPLES 10
arvindnr89 17:8e2824f64b91 83
arvindnr89 17:8e2824f64b91 84
arvindnr89 12:16d1a5390022 85
arvindnr89 10:48f640a54401 86 //*********** Local Function Definations BEGIN **************//
arvindnr89 10:48f640a54401 87 void init_system();
arvindnr89 10:48f640a54401 88 void init_imu();
arvindnr89 10:48f640a54401 89 //*********** Local Function Definations END **************//
arvindnr89 10:48f640a54401 90
csharer 3:2f76ffbc5cef 91 Timer timer;
arvindnr89 12:16d1a5390022 92 int timer_value = 0;
arvindnr89 12:16d1a5390022 93 int timer_old = 0;
arvindnr89 15:d6d7623a17f8 94 float angle = 0;
arvindnr89 16:3a85662fb740 95 float theta = 0;
arvindnr89 16:3a85662fb740 96 float totalTheta = 0;
arvindnr89 16:3a85662fb740 97 float deltaTheta = 0;
arvindnr89 12:16d1a5390022 98 float velocitySamples[VELOCITY_SAMPLES + 1] = {0.0};
csharer 3:2f76ffbc5cef 99
arvindnr89 16:3a85662fb740 100 float target_pos = 0;
arvindnr89 17:8e2824f64b91 101 float pos_error = 0;
arvindnr89 16:3a85662fb740 102
csharer 3:2f76ffbc5cef 103 Serial pc(USBTX, USBRX);
csharer 3:2f76ffbc5cef 104
csharer 4:2512939c10f0 105 // LEDs
csharer 4:2512939c10f0 106 DigitalOut led1(LED1);
csharer 4:2512939c10f0 107 DigitalOut led2(LED2);
csharer 4:2512939c10f0 108 DigitalOut led3(LED3);
csharer 4:2512939c10f0 109 DigitalOut led4(LED4);
csharer 4:2512939c10f0 110
arvindnr89 17:8e2824f64b91 111
arvindnr89 17:8e2824f64b91 112 //Blue = IMU Intr
csharer 4:2512939c10f0 113
arvindnr89 10:48f640a54401 114
arvindnr89 10:48f640a54401 115
arvindnr89 10:48f640a54401 116
arvindnr89 10:48f640a54401 117 //Used to set angle upon startup
arvindnr89 17:8e2824f64b91 118 bool initilizeAngle;
arvindnr89 17:8e2824f64b91 119 bool initilizeTheta;
arvindnr89 11:2553f5798f84 120
csharer 9:fb671fa0c0be 121 // ================================================================
arvindnr89 10:48f640a54401 122 // === IMU Thread ===
csharer 9:fb671fa0c0be 123 // ================================================================
arvindnr89 10:48f640a54401 124 void imu_update_thread(void const *args)
arvindnr89 15:d6d7623a17f8 125 {
arvindnr89 15:d6d7623a17f8 126 float dAngle = 0;
arvindnr89 16:3a85662fb740 127 float dTheta = 0;
arvindnr89 15:d6d7623a17f8 128 float new_angle = 0;
arvindnr89 16:3a85662fb740 129 float new_theta = 0;
arvindnr89 15:d6d7623a17f8 130 long long timeOut = 0;
arvindnr89 15:d6d7623a17f8 131
arvindnr89 15:d6d7623a17f8 132
arvindnr89 11:2553f5798f84 133 pc.printf("Starting IMU Thread..\r\n");
arvindnr89 11:2553f5798f84 134 while (1) {
arvindnr89 11:2553f5798f84 135 osSignalWait(IMU_UPDATE_SIGNAL,osWaitForever);
arvindnr89 17:8e2824f64b91 136 pin_30 = 1;
arvindnr89 11:2553f5798f84 137 if(mpuInterrupt) {
arvindnr89 10:48f640a54401 138 mpuInterrupt = false;
csharer 9:fb671fa0c0be 139 led3 = !led3;
csharer 9:fb671fa0c0be 140 /********************* All IMU Handling DO NOT MODIFY *****************/
csharer 9:fb671fa0c0be 141 //Disable IRQ
csharer 9:fb671fa0c0be 142 //checkpin.rise(NULL);
csharer 9:fb671fa0c0be 143
csharer 9:fb671fa0c0be 144 //reset interrupt flag and get INT_STATUS byte
arvindnr89 10:48f640a54401 145
csharer 9:fb671fa0c0be 146 mpuIntStatus = mpu.getIntStatus();
csharer 9:fb671fa0c0be 147
csharer 9:fb671fa0c0be 148 //get current FIFO count
csharer 9:fb671fa0c0be 149 fifoCount = mpu.getFIFOCount();
csharer 9:fb671fa0c0be 150
csharer 9:fb671fa0c0be 151 // check for overflow (this should never happen unless our code is too inefficient)
csharer 9:fb671fa0c0be 152 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
csharer 9:fb671fa0c0be 153 // reset so we can continue cleanly
csharer 9:fb671fa0c0be 154 mpu.resetFIFO();
arvindnr89 10:48f640a54401 155 pc.printf("MPU FIFO overflow!");
csharer 9:fb671fa0c0be 156 //otherwise, check for DMP data ready interrupt (this should happen frequently)
csharer 9:fb671fa0c0be 157 } else if (mpuIntStatus & 0x02) {
csharer 9:fb671fa0c0be 158 //wait for correct available data length, should be a VERY short wait
arvindnr89 15:d6d7623a17f8 159 timeOut = 0;
arvindnr89 15:d6d7623a17f8 160 while (fifoCount < packetSize) {
arvindnr89 15:d6d7623a17f8 161 timeOut++;
arvindnr89 15:d6d7623a17f8 162 fifoCount = mpu.getFIFOCount();
arvindnr89 15:d6d7623a17f8 163 if(timeOut > 100000000){
arvindnr89 15:d6d7623a17f8 164 break;
arvindnr89 15:d6d7623a17f8 165 }
arvindnr89 15:d6d7623a17f8 166 }
csharer 9:fb671fa0c0be 167
csharer 9:fb671fa0c0be 168 //read a packet from FIFO
csharer 9:fb671fa0c0be 169 mpu.getFIFOBytes(fifoBuffer, packetSize);
csharer 9:fb671fa0c0be 170
csharer 9:fb671fa0c0be 171 //track FIFO count here in case there is > 1 packet available
csharer 9:fb671fa0c0be 172 //(this lets us immediately read more without waiting for an interrupt)
csharer 9:fb671fa0c0be 173 fifoCount -= packetSize;
csharer 9:fb671fa0c0be 174
csharer 9:fb671fa0c0be 175 //Read new angle from IMU
arvindnr89 16:3a85662fb740 176 dmpGetReadings(&new_angle,&new_theta);
arvindnr89 15:d6d7623a17f8 177 new_angle = (float)(new_angle - ANGLE_OFFSET);
arvindnr89 16:3a85662fb740 178 new_theta = float(new_theta + THETA_OFFSET);
arvindnr89 17:8e2824f64b91 179 //pc.printf("Angle: %f\r\n",new_angle);
arvindnr89 17:8e2824f64b91 180 //pc.printf("Theta: %f\r\n",new_theta);
arvindnr89 15:d6d7623a17f8 181
arvindnr89 10:48f640a54401 182 //Calculate the delta angle
csharer 9:fb671fa0c0be 183 dAngle = new_angle - angle;
arvindnr89 16:3a85662fb740 184 dTheta = new_theta - theta;
csharer 9:fb671fa0c0be 185
csharer 9:fb671fa0c0be 186 //Filter out angle readings larger then MAX_ANGLE_DELTA
arvindnr89 10:48f640a54401 187 if(initilizeAngle) {
csharer 9:fb671fa0c0be 188 angle = new_angle;
arvindnr89 10:48f640a54401 189 initilizeAngle = false;
arvindnr89 17:8e2824f64b91 190 pc.printf("\t\t\t Setting Initial Value: %f\r\n",angle);
arvindnr89 10:48f640a54401 191 } else if( ((dAngle < 15) && (dAngle > -15))) {
arvindnr89 10:48f640a54401 192 angle = new_angle;
arvindnr89 16:3a85662fb740 193 theta = new_theta;
csharer 9:fb671fa0c0be 194 } else {
arvindnr89 17:8e2824f64b91 195 pc.printf("\t\t\t Delta Angle too Large: %f. Ignored \r\n",dAngle);
arvindnr89 17:8e2824f64b91 196 pc.printf("\t\t\t New Angle: %f Old Angle: %f\r\n",new_angle,angle);
csharer 9:fb671fa0c0be 197 }
arvindnr89 16:3a85662fb740 198
arvindnr89 16:3a85662fb740 199 if(initilizeTheta) {
arvindnr89 16:3a85662fb740 200 theta = new_theta;
arvindnr89 16:3a85662fb740 201 initilizeTheta = false;
arvindnr89 16:3a85662fb740 202 } else if( ((dTheta < 15) && (dTheta > -15))) {
arvindnr89 16:3a85662fb740 203 theta = new_theta;
arvindnr89 16:3a85662fb740 204 } else {
arvindnr89 17:8e2824f64b91 205 //pc.printf("\t\t\t Delta Theta too Large: %f. Ignored \r\n",dTheta);
arvindnr89 17:8e2824f64b91 206 //pc.printf("\t\t\t New Theta: %f Old Theta: %f\r\n",new_theta,theta);
arvindnr89 16:3a85662fb740 207 theta = new_theta;
arvindnr89 16:3a85662fb740 208 dTheta = dTheta > 0 ? -1 : 1;
arvindnr89 16:3a85662fb740 209 dTheta = 1;
arvindnr89 16:3a85662fb740 210 }
arvindnr89 16:3a85662fb740 211 totalTheta += dTheta;
arvindnr89 16:3a85662fb740 212 //pc.printf("Total Theta: %f\r\n",totalTheta);
csharer 9:fb671fa0c0be 213 }
arvindnr89 15:d6d7623a17f8 214 else{
arvindnr89 17:8e2824f64b91 215 pc.printf("\t\t\t IMU Error. MPU Status: %d!!\r\n",mpuIntStatus);
arvindnr89 15:d6d7623a17f8 216 }
arvindnr89 17:8e2824f64b91 217 pin_30 = 0;
csharer 9:fb671fa0c0be 218 /********************* All IMU Handling DO NOT MODIFY *****************/
arvindnr89 10:48f640a54401 219 }//End of if(mpuInterrupt) loop
arvindnr89 11:2553f5798f84 220 osSignalClear(imu_update_thread_ID,IMU_UPDATE_SIGNAL);
arvindnr89 17:8e2824f64b91 221 osSignalSet(pid_update_thread_ID,PID_UPDATE_SIGNAL);
arvindnr89 10:48f640a54401 222 }
arvindnr89 10:48f640a54401 223 }
csharer 9:fb671fa0c0be 224
arvindnr89 11:2553f5798f84 225 // ================================================================
arvindnr89 11:2553f5798f84 226 // === PID Thread ===
arvindnr89 11:2553f5798f84 227 // ================================================================
arvindnr89 11:2553f5798f84 228 void pid_update_thread(void const *args)
arvindnr89 11:2553f5798f84 229 {
arvindnr89 12:16d1a5390022 230
arvindnr89 12:16d1a5390022 231 float Kp1 = 0;
arvindnr89 12:16d1a5390022 232 float Kd1 = 0;
arvindnr89 12:16d1a5390022 233 float Kp2 = 0;
arvindnr89 12:16d1a5390022 234 float Kd2 = 0;
arvindnr89 12:16d1a5390022 235 float target_angle = 0;
arvindnr89 12:16d1a5390022 236 float target_angle_old = 0;
arvindnr89 12:16d1a5390022 237 float change_in_target_angle = 0;
arvindnr89 12:16d1a5390022 238 float change_in_angle = 0;
arvindnr89 12:16d1a5390022 239 float angle_old1 = 0;
arvindnr89 12:16d1a5390022 240 float angle_old2 = 0;
arvindnr89 12:16d1a5390022 241 float kp_term = 0;
arvindnr89 12:16d1a5390022 242 float kd_term = 0;
arvindnr89 12:16d1a5390022 243 float throttle = 0;
arvindnr89 12:16d1a5390022 244 float steering = 0;
arvindnr89 16:3a85662fb740 245 float steering_output = 0;
arvindnr89 12:16d1a5390022 246 float control_output = 0;
arvindnr89 12:16d1a5390022 247 int robot_pos = 0;
arvindnr89 17:8e2824f64b91 248 //int loop_counter = 0;
arvindnr89 12:16d1a5390022 249 int velocitySamplesCounter = 0;
arvindnr89 12:16d1a5390022 250 int dt = 0;
arvindnr89 12:16d1a5390022 251 float error = 0;
arvindnr89 12:16d1a5390022 252 float velocity = 0;
arvindnr89 12:16d1a5390022 253 bool fallen = true;
arvindnr89 12:16d1a5390022 254
arvindnr89 12:16d1a5390022 255 // --- For Position controller --- //
arvindnr89 16:3a85662fb740 256
arvindnr89 12:16d1a5390022 257 float kp_pos_term = 0;
arvindnr89 12:16d1a5390022 258 float kd_pos_term = 0;
arvindnr89 12:16d1a5390022 259 float change_in_target_pos = 0;
arvindnr89 16:3a85662fb740 260
arvindnr89 12:16d1a5390022 261 float target_pos_old = 0;
csharer 14:94c65d1c8bad 262 float target_velocity = 0;
csharer 14:94c65d1c8bad 263 float velocity_error = 0;
csharer 14:94c65d1c8bad 264 float change_in_velocity = 0;
csharer 14:94c65d1c8bad 265 float d_velocity = 0;
arvindnr89 12:16d1a5390022 266
arvindnr89 12:16d1a5390022 267 float runningSumReplaceVal = 0;
arvindnr89 12:16d1a5390022 268 float newVelocity = 0;
arvindnr89 15:d6d7623a17f8 269 float angle_old = 0;
arvindnr89 12:16d1a5390022 270
arvindnr89 12:16d1a5390022 271 float velocitySum = 0;
arvindnr89 15:d6d7623a17f8 272 float theta_error = 0;
arvindnr89 15:d6d7623a17f8 273 float target_theta = 0;
arvindnr89 12:16d1a5390022 274 memset(velocitySamples,0,sizeof(velocitySamples));
arvindnr89 11:2553f5798f84 275 pc.printf("Starting PID Thread..\r\n");
arvindnr89 11:2553f5798f84 276 while(1) {
arvindnr89 17:8e2824f64b91 277 osSignalWait(PID_UPDATE_SIGNAL,osWaitForever);
arvindnr89 17:8e2824f64b91 278 pin_5 = 1;
arvindnr89 15:d6d7623a17f8 279 //Button Press on the remote initilizes the robot position.
arvindnr89 15:d6d7623a17f8 280 if(button) {
arvindnr89 15:d6d7623a17f8 281 pos_M1 = 0;
arvindnr89 15:d6d7623a17f8 282 pos_M2 = 0;
arvindnr89 15:d6d7623a17f8 283 target_pos = 0;
arvindnr89 15:d6d7623a17f8 284 motor1 = 0;
arvindnr89 15:d6d7623a17f8 285 motor2 = 0;
arvindnr89 15:d6d7623a17f8 286 control_output = 0;
arvindnr89 16:3a85662fb740 287 totalTheta = 0;
arvindnr89 16:3a85662fb740 288 target_theta = 0;
arvindnr89 16:3a85662fb740 289 steering_output = 0;
arvindnr89 15:d6d7623a17f8 290 fallen = false;
arvindnr89 15:d6d7623a17f8 291 }
arvindnr89 11:2553f5798f84 292 //Get the time stamp as soon as it enters the loop
arvindnr89 11:2553f5798f84 293 timer_value = timer.read_us();
arvindnr89 11:2553f5798f84 294 //led1 = led1^1;
arvindnr89 11:2553f5798f84 295 led4 = !fallen;
arvindnr89 11:2553f5798f84 296 led2 = button;
csharer 9:fb671fa0c0be 297
arvindnr89 11:2553f5798f84 298 if(jstick_v > 80)
arvindnr89 11:2553f5798f84 299 led3 = 1;
arvindnr89 11:2553f5798f84 300 else
arvindnr89 11:2553f5798f84 301 led3 = 0;
arvindnr89 11:2553f5798f84 302
arvindnr89 11:2553f5798f84 303 //Preset Knob Values for Green Balance Bot
arvindnr89 12:16d1a5390022 304 //knob1 = 1.132;
arvindnr89 12:16d1a5390022 305 //knob2 = 47.284;
arvindnr89 12:16d1a5390022 306 //knob3 = 18.46;
arvindnr89 12:16d1a5390022 307 //knob4 = 17.07;
arvindnr89 11:2553f5798f84 308
arvindnr89 11:2553f5798f84 309 //Set Gainz with knobs
arvindnr89 15:d6d7623a17f8 310 Kp1 = ((float)knob1);// / 100.0;
arvindnr89 15:d6d7623a17f8 311 Kd1 = ((float)knob2);// / 1.0;
arvindnr89 15:d6d7623a17f8 312 Kp2 = ((float)knob3);// / 1000.0;
arvindnr89 15:d6d7623a17f8 313 Kd2 = ((float)knob4);// / 100.0;
arvindnr89 11:2553f5798f84 314
arvindnr89 11:2553f5798f84 315 //Joystick control
arvindnr89 16:3a85662fb740 316 //throttle = (float)jstick_v / THROTTLE_DAMPNER;
arvindnr89 16:3a85662fb740 317 //steering = (float)jstick_h / STREEING_DAMPNER;
arvindnr89 11:2553f5798f84 318
arvindnr89 11:2553f5798f84 319 //Calculate the delta time
arvindnr89 11:2553f5798f84 320 dt = (timer_value - timer_old);
arvindnr89 11:2553f5798f84 321 timer_old = timer_value;
arvindnr89 11:2553f5798f84 322 angle_old = angle;
arvindnr89 17:8e2824f64b91 323
arvindnr89 11:2553f5798f84 324 // STANDING: Motor Control Enabled
arvindnr89 11:2553f5798f84 325 //******************** PID Control BEGIN ********************** //
arvindnr89 11:2553f5798f84 326 if(((angle < 45) && (angle > -45)) && (fallen == false)) {
arvindnr89 17:8e2824f64b91 327 //if(0){
arvindnr89 17:8e2824f64b91 328 //loop_counter++;
arvindnr89 11:2553f5798f84 329 //CS Pd Target Angle Contoller Goes Here
arvindnr89 11:2553f5798f84 330
arvindnr89 11:2553f5798f84 331 //Robot Position
arvindnr89 11:2553f5798f84 332 robot_pos = (pos_M1 + pos_M2) / 2;
arvindnr89 16:3a85662fb740 333 //target_pos += throttle/2;
arvindnr89 12:16d1a5390022 334
arvindnr89 12:16d1a5390022 335 //Velocity Computation
arvindnr89 12:16d1a5390022 336 velocitySamplesCounter++;
arvindnr89 12:16d1a5390022 337 if(velocitySamplesCounter == VELOCITY_SAMPLES){
arvindnr89 12:16d1a5390022 338 velocitySamplesCounter = 0;
arvindnr89 12:16d1a5390022 339 }
arvindnr89 12:16d1a5390022 340 runningSumReplaceVal = velocitySamples[velocitySamplesCounter]; //value to replace
arvindnr89 12:16d1a5390022 341 newVelocity = (motor1 + motor2) / 2;
arvindnr89 12:16d1a5390022 342 velocitySamples[velocitySamplesCounter] = newVelocity; //replace value with
arvindnr89 12:16d1a5390022 343 velocitySum = velocitySum - runningSumReplaceVal + newVelocity;
arvindnr89 12:16d1a5390022 344 velocity = velocitySum/VELOCITY_SAMPLES;
csharer 14:94c65d1c8bad 345
arvindnr89 15:d6d7623a17f8 346 // ***************** Angular Controller *********************
arvindnr89 16:3a85662fb740 347
arvindnr89 17:8e2824f64b91 348 float Kp1A = 0;//Kp1/100;
arvindnr89 16:3a85662fb740 349 target_theta += steering;
arvindnr89 16:3a85662fb740 350 theta_error = totalTheta - target_theta;
arvindnr89 16:3a85662fb740 351 steering_output = -Kp1A * theta_error;
arvindnr89 15:d6d7623a17f8 352
arvindnr89 15:d6d7623a17f8 353 // ***************** Position Controller *********************
arvindnr89 17:8e2824f64b91 354 float Kp1P = Kp2/100;
arvindnr89 17:8e2824f64b91 355 float Kd1P = Kd2;
arvindnr89 17:8e2824f64b91 356 //Kp1P = ;//5.73/100;
csharer 14:94c65d1c8bad 357 //target_pos = 0;
arvindnr89 11:2553f5798f84 358 pos_error = robot_pos - target_pos; //robot_pos - target_pos;
arvindnr89 11:2553f5798f84 359
arvindnr89 11:2553f5798f84 360 //KP Term
arvindnr89 15:d6d7623a17f8 361 kp_pos_term = Kp1P * pos_error;
arvindnr89 11:2553f5798f84 362
arvindnr89 11:2553f5798f84 363 //KD Term
arvindnr89 11:2553f5798f84 364 change_in_target_pos = target_pos - target_pos_old;
arvindnr89 11:2553f5798f84 365 //kd_pos_term = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
arvindnr89 15:d6d7623a17f8 366 kd_pos_term = ((Kd1P * change_in_target_pos) + (Kd1P*velocity));
csharer 14:94c65d1c8bad 367 target_velocity = kp_pos_term; // + kd_pos_term;
csharer 14:94c65d1c8bad 368 target_velocity = CAP(target_velocity, 100);
csharer 14:94c65d1c8bad 369
csharer 14:94c65d1c8bad 370 //CS ***************** Velocity Controller *********************
arvindnr89 17:8e2824f64b91 371 float Kp2V = Kp2/100;
arvindnr89 17:8e2824f64b91 372 float Kd2V = Kd2;
arvindnr89 17:8e2824f64b91 373 Kp2V = 31.81/100;
arvindnr89 17:8e2824f64b91 374 Kd2V = 17.5;
csharer 14:94c65d1c8bad 375 //target_velocity = throttle;
csharer 14:94c65d1c8bad 376 velocity_error = target_velocity - velocity;
arvindnr89 17:8e2824f64b91 377 change_in_velocity = velocity - velocitySamples[(velocitySamplesCounter + 5)%VELOCITY_SAMPLES];
arvindnr89 15:d6d7623a17f8 378 d_velocity = (Kd2V * change_in_velocity)/dt;
arvindnr89 12:16d1a5390022 379
arvindnr89 17:8e2824f64b91 380 target_angle = (-velocity_error * Kp2V)+ d_velocity;
arvindnr89 11:2553f5798f84 381 //CS ************ PD Stability CONTROLLER HERE ****************
arvindnr89 17:8e2824f64b91 382 float Kp1S = Kp1/10; //7.373/1000.0;
arvindnr89 17:8e2824f64b91 383 float Kd1S = Kd1*100; //50.986/1.0;
arvindnr89 17:8e2824f64b91 384 // Kp1S = 3.48/1000;
arvindnr89 17:8e2824f64b91 385 //Kd1S = 44.5;
arvindnr89 15:d6d7623a17f8 386
arvindnr89 11:2553f5798f84 387 error = target_angle - angle;
csharer 14:94c65d1c8bad 388 kp_term = Kp1S * error;
arvindnr89 11:2553f5798f84 389
arvindnr89 11:2553f5798f84 390 change_in_target_angle = target_angle - target_angle_old; //add
arvindnr89 11:2553f5798f84 391 change_in_angle = angle - angle_old2; //add
csharer 14:94c65d1c8bad 392 kd_term = ((Kd1S * change_in_target_angle) - (Kd1S*change_in_angle)) / dt;
arvindnr89 11:2553f5798f84 393 //kd_term = ((Kd1 * change_in_target_angle) - (Kd1*velocity));
arvindnr89 11:2553f5798f84 394
arvindnr89 11:2553f5798f84 395 //pc.printf("dAngle:%f\r\n", angle-angle_old1);
arvindnr89 11:2553f5798f84 396
arvindnr89 11:2553f5798f84 397 //Control Output
arvindnr89 12:16d1a5390022 398 control_output += kp_term + kd_term; //-100 to 100
arvindnr89 11:2553f5798f84 399 control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
arvindnr89 16:3a85662fb740 400 motor1 = (int16_t)(control_output + (steering_output));
arvindnr89 16:3a85662fb740 401 motor2 = (int16_t)(control_output - (steering_output));
arvindnr89 11:2553f5798f84 402 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
arvindnr89 11:2553f5798f84 403 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
arvindnr89 11:2553f5798f84 404
arvindnr89 11:2553f5798f84 405 //Update variables
arvindnr89 11:2553f5798f84 406 target_angle_old = target_angle;
arvindnr89 11:2553f5798f84 407 angle_old2 = angle_old1;
arvindnr89 11:2553f5798f84 408 angle_old1 = angle;
arvindnr89 11:2553f5798f84 409
csharer 13:a18d41a60d0b 410 //Enable Motors
arvindnr89 11:2553f5798f84 411 enable = ENABLE;
arvindnr89 11:2553f5798f84 412 setMotor1Speed(-motor1);
arvindnr89 11:2553f5798f84 413 setMotor2Speed(-motor2);
arvindnr89 12:16d1a5390022 414 //robot_pos += (-motor1 + -motor2) / 2;
arvindnr89 11:2553f5798f84 415 //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);
arvindnr89 16:3a85662fb740 416
arvindnr89 16:3a85662fb740 417 if(abs(motor1) == MAX_CONTROL_OUTPUT || abs(motor2) == MAX_CONTROL_OUTPUT) {
arvindnr89 17:8e2824f64b91 418 pc.printf("Max Speed Reached. Killing the Robot\n");
arvindnr89 17:8e2824f64b91 419 enable = DISABLE;
arvindnr89 16:3a85662fb740 420 fallen = true;
arvindnr89 16:3a85662fb740 421 }
arvindnr89 11:2553f5798f84 422 } else { //[FALLEN}
arvindnr89 11:2553f5798f84 423 //Disable Motors
arvindnr89 11:2553f5798f84 424 enable = DISABLE;
arvindnr89 11:2553f5798f84 425
arvindnr89 11:2553f5798f84 426 //Set fallen flag
arvindnr89 11:2553f5798f84 427 fallen = true;
arvindnr89 11:2553f5798f84 428 }
arvindnr89 17:8e2824f64b91 429 /*if(loop_counter >= 20) {
arvindnr89 11:2553f5798f84 430 loop_counter = 0;
arvindnr89 15:d6d7623a17f8 431 //pc.printf("Target Vel: %f\r\n",target_velocity);
arvindnr89 11:2553f5798f84 432 //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);
arvindnr89 11:2553f5798f84 433 //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);
arvindnr89 11:2553f5798f84 434 //pc.printf("CAngle: %d, TAngle: %d,Pos: %d, dt: %d\r\n",(int)angle,target_angle,robot_pos,dt);
arvindnr89 11:2553f5798f84 435 //wait_us(200);
arvindnr89 17:8e2824f64b91 436 }*/
arvindnr89 17:8e2824f64b91 437 pin_5 = 0;
arvindnr89 17:8e2824f64b91 438 osSignalClear(pid_update_thread_ID,PID_UPDATE_SIGNAL);
arvindnr89 17:8e2824f64b91 439 //osSignalSet(communication_update_thread_ID,COMMUNIATION_UPDATE_SIGNAL);
arvindnr89 11:2553f5798f84 440 } //end main loop
arvindnr89 11:2553f5798f84 441
arvindnr89 11:2553f5798f84 442 }
arvindnr89 11:2553f5798f84 443
arvindnr89 11:2553f5798f84 444 // ================================================================
arvindnr89 11:2553f5798f84 445 // === Communication Thread ===
arvindnr89 11:2553f5798f84 446 // ================================================================
arvindnr89 11:2553f5798f84 447 void communication_update_thread(void const *args)
arvindnr89 11:2553f5798f84 448 {
arvindnr89 11:2553f5798f84 449 pc.printf("Starting Communication Thread..\r\n");
arvindnr89 11:2553f5798f84 450 while(1) {
arvindnr89 17:8e2824f64b91 451 //osSignalWait(COMMUNIATION_UPDATE_SIGNAL,osWaitForever);
arvindnr89 11:2553f5798f84 452 //Recieve Data
arvindnr89 17:8e2824f64b91 453 pin_6 = 1;
arvindnr89 11:2553f5798f84 454 rxLen = rf_receive(rxBuffer, 128);
arvindnr89 11:2553f5798f84 455 if(rxLen > 0) {
arvindnr89 11:2553f5798f84 456 led1 = led1^1;
arvindnr89 11:2553f5798f84 457 //Process data with our protocal
arvindnr89 11:2553f5798f84 458 communication_protocal(rxLen);
arvindnr89 11:2553f5798f84 459 }
arvindnr89 17:8e2824f64b91 460 pin_6 = 0;
arvindnr89 17:8e2824f64b91 461 //osSignalClear(communication_update_thread_ID,COMMUNIATION_UPDATE_SIGNAL);
arvindnr89 11:2553f5798f84 462 }
arvindnr89 11:2553f5798f84 463 }
arvindnr89 10:48f640a54401 464
csharer 3:2f76ffbc5cef 465 // ================================================================
csharer 3:2f76ffbc5cef 466 // === INITIAL SETUP ===
csharer 3:2f76ffbc5cef 467 // ================================================================
csharer 3:2f76ffbc5cef 468 void init_imu()
csharer 3:2f76ffbc5cef 469 {
arvindnr89 10:48f640a54401 470 pc.printf("Start IMU Initilization.. \r\n");
csharer 3:2f76ffbc5cef 471
csharer 3:2f76ffbc5cef 472 // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
csharer 3:2f76ffbc5cef 473 mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
csharer 3:2f76ffbc5cef 474 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
csharer 3:2f76ffbc5cef 475 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
csharer 3:2f76ffbc5cef 476 mpu.setDLPFMode(MPU6050_DLPF_BW_10); //10,20,42,98,188 // Default factor for BROBOT:10
arvindnr89 17:8e2824f64b91 477 mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
csharer 3:2f76ffbc5cef 478 mpu.setSleepEnabled(false);
csharer 3:2f76ffbc5cef 479 wait_ms(500);
csharer 3:2f76ffbc5cef 480
csharer 3:2f76ffbc5cef 481 // load and configure the DMP
csharer 3:2f76ffbc5cef 482 devStatus = mpu.dmpInitialize();
csharer 3:2f76ffbc5cef 483 if(devStatus == 0) {
csharer 3:2f76ffbc5cef 484 mpu.setDMPEnabled(true);
csharer 3:2f76ffbc5cef 485 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 486 dmpReady = true;
csharer 4:2512939c10f0 487 } else {
csharer 3:2f76ffbc5cef 488 // 1 = initial memory load failed
csharer 3:2f76ffbc5cef 489 // 2 = DMP configuration updates failed
csharer 3:2f76ffbc5cef 490 pc.printf("DMP INIT error \r\n");
csharer 3:2f76ffbc5cef 491 }
csharer 3:2f76ffbc5cef 492
csharer 3:2f76ffbc5cef 493 //Gyro Calibration
csharer 3:2f76ffbc5cef 494 wait_ms(500);
arvindnr89 10:48f640a54401 495 pc.printf("Gyro calibration!! Dont move the robot in 1 second... \r\n");
csharer 3:2f76ffbc5cef 496 wait_ms(500);
csharer 4:2512939c10f0 497
csharer 3:2f76ffbc5cef 498 // verify connection
csharer 3:2f76ffbc5cef 499 pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");
csharer 3:2f76ffbc5cef 500
csharer 3:2f76ffbc5cef 501 //Adjust Sensor Fusion Gain
csharer 3:2f76ffbc5cef 502 dmpSetSensorFusionAccelGain(0x20);
csharer 3:2f76ffbc5cef 503
csharer 3:2f76ffbc5cef 504 wait_ms(200);
csharer 3:2f76ffbc5cef 505 mpu.resetFIFO();
csharer 3:2f76ffbc5cef 506 }
csharer 3:2f76ffbc5cef 507
arvindnr89 10:48f640a54401 508 void init_system()
arvindnr89 10:48f640a54401 509 {
arvindnr89 10:48f640a54401 510 initilizeAngle = true;
arvindnr89 17:8e2824f64b91 511 initilizeTheta = true;
arvindnr89 16:3a85662fb740 512 totalTheta = 0;
arvindnr89 16:3a85662fb740 513 target_pos = 0;
arvindnr89 16:3a85662fb740 514 pos_error = 0;
arvindnr89 10:48f640a54401 515 //Set the Channel. 0 is default, 15 is max
arvindnr89 10:48f640a54401 516 mrf.SetChannel(MRF_CHANNEL);
arvindnr89 10:48f640a54401 517 enable = DISABLE; //Disable Motors
arvindnr89 10:48f640a54401 518 pc.baud(115200);
arvindnr89 10:48f640a54401 519 pc.printf("Booting BroBot mbed RTOS..\r\n");
arvindnr89 10:48f640a54401 520
arvindnr89 10:48f640a54401 521
arvindnr89 10:48f640a54401 522 //Initilize the IMU
arvindnr89 10:48f640a54401 523 init_imu();
arvindnr89 10:48f640a54401 524 //Attach Interupt for IMU on rising edge of checkpin
arvindnr89 10:48f640a54401 525 checkpin.rise(&dmpDataReady);
arvindnr89 10:48f640a54401 526 pc.printf("IMU Initilized..\r\n");
arvindnr89 10:48f640a54401 527
arvindnr89 10:48f640a54401 528 //Init Stepper Motors
arvindnr89 10:48f640a54401 529 //Attach Motor Control Timer Call Back Functions
arvindnr89 10:48f640a54401 530 timer_M1.attach_us(&ISR1, ZERO_SPEED);//1s Period
arvindnr89 10:48f640a54401 531 timer_M2.attach_us(&ISR2, ZERO_SPEED);//1s Period
arvindnr89 10:48f640a54401 532 step_M1 = 1;
arvindnr89 10:48f640a54401 533 dir_M1 = 1;
arvindnr89 10:48f640a54401 534 pc.printf("Motors Initilized..\r\n");
arvindnr89 10:48f640a54401 535
arvindnr89 10:48f640a54401 536 //Timers initilized
arvindnr89 10:48f640a54401 537 timer.start();
arvindnr89 10:48f640a54401 538 timer_value = timer.read_us();
arvindnr89 10:48f640a54401 539
arvindnr89 10:48f640a54401 540 //Init GPIO Monitors
arvindnr89 17:8e2824f64b91 541
arvindnr89 10:48f640a54401 542
arvindnr89 11:2553f5798f84 543 enable = ENABLE; //Enable Motors
arvindnr89 10:48f640a54401 544 }
arvindnr89 10:48f640a54401 545
csharer 3:2f76ffbc5cef 546 // ================================================================
csharer 3:2f76ffbc5cef 547 // === MAIN PROGRAM LOOP ===
csharer 3:2f76ffbc5cef 548 // ================================================================
arvindnr89 11:2553f5798f84 549
csharer 4:2512939c10f0 550
csharer 3:2f76ffbc5cef 551 int main()
csharer 3:2f76ffbc5cef 552 {
csharer 9:fb671fa0c0be 553
arvindnr89 10:48f640a54401 554 init_system();
csharer 4:2512939c10f0 555
arvindnr89 10:48f640a54401 556 //Create IMU Thread
arvindnr89 11:2553f5798f84 557 imu_update_thread_ID = osThreadCreate(osThread(imu_update_thread), NULL);
csharer 9:fb671fa0c0be 558
arvindnr89 11:2553f5798f84 559 //Create PID Thread
arvindnr89 11:2553f5798f84 560 pid_update_thread_ID = osThreadCreate(osThread(pid_update_thread), NULL);
csharer 9:fb671fa0c0be 561
arvindnr89 11:2553f5798f84 562 //Create Communication Thread
arvindnr89 11:2553f5798f84 563 communication_update_thread_ID = osThreadCreate(osThread(communication_update_thread), NULL);
arvindnr89 16:3a85662fb740 564
csharer 4:2512939c10f0 565
csharer 3:2f76ffbc5cef 566 } //End Main()