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@20:a7cba632d0b1, 2017-03-22 (annotated)

Committer:
csharer
Date:
Wed Mar 22 21:59:10 2017 +0000
Revision:
20:a7cba632d0b1
Parent:
19:19d72dc64b43 
Final Skeleton code for ESE350

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 9:fb671fa0c0be 6 /******************************* README USAGE *******************************
csharer 9:fb671fa0c0be 7 * This robot must be powered on while it is laying down flat on a still table
csharer 9:fb671fa0c0be 8 * This allows the robot to calibrate the IMU (~5 seconds)
csharer 9:fb671fa0c0be 9 * The motors are DISABLED when the robot tilts more then +-45 degrees from
csharer 19:19d72dc64b43 10 * vertical. To ENABLE the motors you must lift the robot to 0 degres and
csharer 9:fb671fa0c0be 11 * press the joystick button.
csharer 9:fb671fa0c0be 12 * To reset the motor positions you must press the josystick button anytime.
csharer 9:fb671fa0c0be 13 ******************************************************************************/
csharer 3:2f76ffbc5cef 14
arvindnr89 10:48f640a54401 15 //Controller Values
arvindnr89 10:48f640a54401 16 float knob1, knob2, knob3, knob4;
arvindnr89 10:48f640a54401 17 float jstick_h, jstick_v;
arvindnr89 10:48f640a54401 18
arvindnr89 10:48f640a54401 19 //Button
arvindnr89 10:48f640a54401 20 bool button;
arvindnr89 10:48f640a54401 21
csharer 4:2512939c10f0 22 //BroBot Begin
csharer 9:fb671fa0c0be 23 #include "cmsis_os.h"
arvindnr89 11:2553f5798f84 24 #include "rtos_definations.h"
csharer 3:2f76ffbc5cef 25 #include "pin_assignments.h"
csharer 3:2f76ffbc5cef 26 #include "I2Cdev.h"
csharer 3:2f76ffbc5cef 27 #include "JJ_MPU6050_DMP_6Axis.h"
csharer 3:2f76ffbc5cef 28 #include "BroBot.h"
csharer 3:2f76ffbc5cef 29 #include "BroBot_IMU.h"
csharer 3:2f76ffbc5cef 30 #include "stepper_motors.h"
csharer 4:2512939c10f0 31 #include "MRF24J40.h"
arvindnr89 10:48f640a54401 32 #include "communication.h"
csharer 4:2512939c10f0 33
csharer 9:fb671fa0c0be 34 //Angle Offset is used to set the natural balance point of your robot.
csharer 18:ee252b8f7430 35 //You should adjust this offset so that your robots balance point is near 0
csharer 19:19d72dc64b43 36 #define ANGLE_OFFSET 107 //THIS NEEDS TO BE CHANGED FOR YOUR ROBOT
csharer 19:19d72dc64b43 37 #define THETA_OFFSET 0
csharer 18:ee252b8f7430 38
csharer 18:ee252b8f7430 39 //Set Channel to (Group Number - 1). Possible Channels are 0-15
arvindnr89 10:48f640a54401 40 #define MRF_CHANNEL 2
csharer 6:62cdb7482b50 41
csharer 4:2512939c10f0 42 //Knobs
csharer 4:2512939c10f0 43 #define POT1 p17
csharer 4:2512939c10f0 44 #define POT2 p18
csharer 4:2512939c10f0 45 #define POT3 p16
csharer 4:2512939c10f0 46 #define POT4 p15
csharer 4:2512939c10f0 47 //JoyStick
csharer 4:2512939c10f0 48 #define POTV p19
csharer 4:2512939c10f0 49 #define POTH p20
csharer 3:2f76ffbc5cef 50
csharer 3:2f76ffbc5cef 51 //PID
csharer 3:2f76ffbc5cef 52 #define MAX_THROTTLE 580
csharer 3:2f76ffbc5cef 53 #define MAX_STEERING 150
csharer 19:19d72dc64b43 54 #define MAX_TARGET_ANGLE 12
csharer 3:2f76ffbc5cef 55
csharer 19:19d72dc64b43 56 //Joystick Control
csharer 19:19d72dc64b43 57 #define THROTTLE_DAMPNER 30
csharer 19:19d72dc64b43 58 #define STREEING_DAMPNER 10
arvindnr89 17:8e2824f64b91 59 #define VELOCITY_SAMPLES 10
arvindnr89 17:8e2824f64b91 60
arvindnr89 10:48f640a54401 61 //*********** Local Function Definations BEGIN **************//
arvindnr89 10:48f640a54401 62 void init_system();
arvindnr89 10:48f640a54401 63 void init_imu();
arvindnr89 10:48f640a54401 64 //*********** Local Function Definations END **************//
arvindnr89 10:48f640a54401 65
csharer 3:2f76ffbc5cef 66 Timer timer;
arvindnr89 12:16d1a5390022 67 int timer_value = 0;
arvindnr89 12:16d1a5390022 68 int timer_old = 0;
arvindnr89 15:d6d7623a17f8 69 float angle = 0;
arvindnr89 16:3a85662fb740 70 float theta = 0;
arvindnr89 16:3a85662fb740 71 float totalTheta = 0;
arvindnr89 16:3a85662fb740 72 float deltaTheta = 0;
arvindnr89 12:16d1a5390022 73 float velocitySamples[VELOCITY_SAMPLES + 1] = {0.0};
csharer 3:2f76ffbc5cef 74
arvindnr89 16:3a85662fb740 75 float target_pos = 0;
arvindnr89 17:8e2824f64b91 76 float pos_error = 0;
arvindnr89 16:3a85662fb740 77
csharer 3:2f76ffbc5cef 78 Serial pc(USBTX, USBRX);
csharer 3:2f76ffbc5cef 79
csharer 4:2512939c10f0 80 // LEDs
csharer 4:2512939c10f0 81 DigitalOut led1(LED1);
csharer 4:2512939c10f0 82 DigitalOut led2(LED2);
csharer 4:2512939c10f0 83 DigitalOut led3(LED3);
csharer 4:2512939c10f0 84 DigitalOut led4(LED4);
csharer 4:2512939c10f0 85
arvindnr89 10:48f640a54401 86 //Used to set angle upon startup
arvindnr89 17:8e2824f64b91 87 bool initilizeAngle;
arvindnr89 17:8e2824f64b91 88 bool initilizeTheta;
arvindnr89 11:2553f5798f84 89
csharer 9:fb671fa0c0be 90 // ================================================================
arvindnr89 10:48f640a54401 91 // === IMU Thread ===
csharer 9:fb671fa0c0be 92 // ================================================================
arvindnr89 10:48f640a54401 93 void imu_update_thread(void const *args)
arvindnr89 15:d6d7623a17f8 94 {
arvindnr89 15:d6d7623a17f8 95 float dAngle = 0;
arvindnr89 16:3a85662fb740 96 float dTheta = 0;
arvindnr89 15:d6d7623a17f8 97 float new_angle = 0;
arvindnr89 16:3a85662fb740 98 float new_theta = 0;
arvindnr89 15:d6d7623a17f8 99 long long timeOut = 0;
arvindnr89 15:d6d7623a17f8 100
arvindnr89 11:2553f5798f84 101 pc.printf("Starting IMU Thread..\r\n");
arvindnr89 11:2553f5798f84 102 while (1) {
arvindnr89 11:2553f5798f84 103 osSignalWait(IMU_UPDATE_SIGNAL,osWaitForever);
arvindnr89 11:2553f5798f84 104 if(mpuInterrupt) {
arvindnr89 10:48f640a54401 105 mpuInterrupt = false;
csharer 9:fb671fa0c0be 106 led3 = !led3;
csharer 9:fb671fa0c0be 107 /********************* All IMU Handling DO NOT MODIFY *****************/
csharer 9:fb671fa0c0be 108 mpuIntStatus = mpu.getIntStatus();
csharer 9:fb671fa0c0be 109
csharer 9:fb671fa0c0be 110 //get current FIFO count
csharer 9:fb671fa0c0be 111 fifoCount = mpu.getFIFOCount();
csharer 9:fb671fa0c0be 112
csharer 9:fb671fa0c0be 113 // check for overflow (this should never happen unless our code is too inefficient)
csharer 9:fb671fa0c0be 114 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
csharer 9:fb671fa0c0be 115 // reset so we can continue cleanly
csharer 9:fb671fa0c0be 116 mpu.resetFIFO();
arvindnr89 10:48f640a54401 117 pc.printf("MPU FIFO overflow!");
csharer 9:fb671fa0c0be 118 //otherwise, check for DMP data ready interrupt (this should happen frequently)
csharer 9:fb671fa0c0be 119 } else if (mpuIntStatus & 0x02) {
csharer 9:fb671fa0c0be 120 //wait for correct available data length, should be a VERY short wait
arvindnr89 15:d6d7623a17f8 121 timeOut = 0;
arvindnr89 15:d6d7623a17f8 122 while (fifoCount < packetSize) {
arvindnr89 15:d6d7623a17f8 123 timeOut++;
arvindnr89 15:d6d7623a17f8 124 fifoCount = mpu.getFIFOCount();
arvindnr89 15:d6d7623a17f8 125 if(timeOut > 100000000){
arvindnr89 15:d6d7623a17f8 126 break;
arvindnr89 15:d6d7623a17f8 127 }
arvindnr89 15:d6d7623a17f8 128 }
csharer 9:fb671fa0c0be 129
csharer 9:fb671fa0c0be 130 //read a packet from FIFO
csharer 9:fb671fa0c0be 131 mpu.getFIFOBytes(fifoBuffer, packetSize);
csharer 9:fb671fa0c0be 132
csharer 9:fb671fa0c0be 133 //track FIFO count here in case there is > 1 packet available
csharer 9:fb671fa0c0be 134 //(this lets us immediately read more without waiting for an interrupt)
csharer 9:fb671fa0c0be 135 fifoCount -= packetSize;
csharer 9:fb671fa0c0be 136
csharer 9:fb671fa0c0be 137 //Read new angle from IMU
arvindnr89 16:3a85662fb740 138 dmpGetReadings(&new_angle,&new_theta);
arvindnr89 15:d6d7623a17f8 139 new_angle = (float)(new_angle - ANGLE_OFFSET);
arvindnr89 16:3a85662fb740 140 new_theta = float(new_theta + THETA_OFFSET);
arvindnr89 15:d6d7623a17f8 141
arvindnr89 10:48f640a54401 142 //Calculate the delta angle
csharer 9:fb671fa0c0be 143 dAngle = new_angle - angle;
arvindnr89 16:3a85662fb740 144 dTheta = new_theta - theta;
csharer 9:fb671fa0c0be 145
csharer 9:fb671fa0c0be 146 //Filter out angle readings larger then MAX_ANGLE_DELTA
arvindnr89 10:48f640a54401 147 if(initilizeAngle) {
csharer 9:fb671fa0c0be 148 angle = new_angle;
arvindnr89 10:48f640a54401 149 initilizeAngle = false;
arvindnr89 17:8e2824f64b91 150 pc.printf("\t\t\t Setting Initial Value: %f\r\n",angle);
arvindnr89 10:48f640a54401 151 } else if( ((dAngle < 15) && (dAngle > -15))) {
arvindnr89 10:48f640a54401 152 angle = new_angle;
arvindnr89 16:3a85662fb740 153 theta = new_theta;
csharer 9:fb671fa0c0be 154 } else {
arvindnr89 17:8e2824f64b91 155 pc.printf("\t\t\t Delta Angle too Large: %f. Ignored \r\n",dAngle);
arvindnr89 17:8e2824f64b91 156 pc.printf("\t\t\t New Angle: %f Old Angle: %f\r\n",new_angle,angle);
csharer 9:fb671fa0c0be 157 }
arvindnr89 16:3a85662fb740 158
arvindnr89 16:3a85662fb740 159 if(initilizeTheta) {
arvindnr89 16:3a85662fb740 160 theta = new_theta;
arvindnr89 16:3a85662fb740 161 initilizeTheta = false;
arvindnr89 16:3a85662fb740 162 } else if( ((dTheta < 15) && (dTheta > -15))) {
arvindnr89 16:3a85662fb740 163 theta = new_theta;
arvindnr89 16:3a85662fb740 164 } else {
arvindnr89 17:8e2824f64b91 165 //pc.printf("\t\t\t Delta Theta too Large: %f. Ignored \r\n",dTheta);
arvindnr89 17:8e2824f64b91 166 //pc.printf("\t\t\t New Theta: %f Old Theta: %f\r\n",new_theta,theta);
arvindnr89 16:3a85662fb740 167 theta = new_theta;
arvindnr89 16:3a85662fb740 168 dTheta = dTheta > 0 ? -1 : 1;
arvindnr89 16:3a85662fb740 169 dTheta = 1;
arvindnr89 16:3a85662fb740 170 }
arvindnr89 16:3a85662fb740 171 totalTheta += dTheta;
csharer 9:fb671fa0c0be 172 }
arvindnr89 15:d6d7623a17f8 173 else{
arvindnr89 17:8e2824f64b91 174 pc.printf("\t\t\t IMU Error. MPU Status: %d!!\r\n",mpuIntStatus);
arvindnr89 15:d6d7623a17f8 175 }
csharer 9:fb671fa0c0be 176 /********************* All IMU Handling DO NOT MODIFY *****************/
arvindnr89 10:48f640a54401 177 }//End of if(mpuInterrupt) loop
arvindnr89 11:2553f5798f84 178 osSignalClear(imu_update_thread_ID,IMU_UPDATE_SIGNAL);
arvindnr89 17:8e2824f64b91 179 osSignalSet(pid_update_thread_ID,PID_UPDATE_SIGNAL);
arvindnr89 10:48f640a54401 180 }
arvindnr89 10:48f640a54401 181 }
csharer 9:fb671fa0c0be 182
arvindnr89 11:2553f5798f84 183 // ================================================================
arvindnr89 11:2553f5798f84 184 // === PID Thread ===
arvindnr89 11:2553f5798f84 185 // ================================================================
arvindnr89 11:2553f5798f84 186 void pid_update_thread(void const *args)
arvindnr89 11:2553f5798f84 187 {
arvindnr89 12:16d1a5390022 188 float Kp1 = 0;
arvindnr89 12:16d1a5390022 189 float Kd1 = 0;
arvindnr89 12:16d1a5390022 190 float Kp2 = 0;
arvindnr89 12:16d1a5390022 191 float Kd2 = 0;
arvindnr89 12:16d1a5390022 192 float target_angle = 0;
arvindnr89 12:16d1a5390022 193 float target_angle_old = 0;
arvindnr89 12:16d1a5390022 194 float change_in_target_angle = 0;
arvindnr89 12:16d1a5390022 195 float change_in_angle = 0;
arvindnr89 12:16d1a5390022 196 float angle_old1 = 0;
arvindnr89 12:16d1a5390022 197 float angle_old2 = 0;
arvindnr89 12:16d1a5390022 198 float kp_term = 0;
arvindnr89 12:16d1a5390022 199 float kd_term = 0;
arvindnr89 12:16d1a5390022 200 float throttle = 0;
arvindnr89 12:16d1a5390022 201 float steering = 0;
arvindnr89 12:16d1a5390022 202 float control_output = 0;
csharer 20:a7cba632d0b1 203 float pos_control_output = 0;
arvindnr89 12:16d1a5390022 204 int robot_pos = 0;
arvindnr89 17:8e2824f64b91 205 //int loop_counter = 0;
arvindnr89 12:16d1a5390022 206 int velocitySamplesCounter = 0;
arvindnr89 12:16d1a5390022 207 int dt = 0;
arvindnr89 12:16d1a5390022 208 float error = 0;
arvindnr89 12:16d1a5390022 209 float velocity = 0;
arvindnr89 12:16d1a5390022 210 bool fallen = true;
arvindnr89 12:16d1a5390022 211
arvindnr89 12:16d1a5390022 212 // --- For Position controller --- //
arvindnr89 12:16d1a5390022 213 float kp_pos_term = 0;
arvindnr89 12:16d1a5390022 214 float kd_pos_term = 0;
arvindnr89 12:16d1a5390022 215 float change_in_target_pos = 0;
arvindnr89 16:3a85662fb740 216
csharer 18:ee252b8f7430 217 // -- For Velocity Controller --- //
arvindnr89 12:16d1a5390022 218 float target_pos_old = 0;
csharer 14:94c65d1c8bad 219 float target_velocity = 0;
arvindnr89 12:16d1a5390022 220 float runningSumReplaceVal = 0;
arvindnr89 12:16d1a5390022 221 float newVelocity = 0;
arvindnr89 12:16d1a5390022 222 float velocitySum = 0;
arvindnr89 12:16d1a5390022 223 memset(velocitySamples,0,sizeof(velocitySamples));
csharer 18:ee252b8f7430 224
arvindnr89 11:2553f5798f84 225 pc.printf("Starting PID Thread..\r\n");
arvindnr89 11:2553f5798f84 226 while(1) {
arvindnr89 17:8e2824f64b91 227 osSignalWait(PID_UPDATE_SIGNAL,osWaitForever);
csharer 18:ee252b8f7430 228
csharer 18:ee252b8f7430 229 //Button Press on the remote initilizes the robot position.
arvindnr89 15:d6d7623a17f8 230 if(button) {
arvindnr89 15:d6d7623a17f8 231 pos_M1 = 0;
arvindnr89 15:d6d7623a17f8 232 pos_M2 = 0;
arvindnr89 15:d6d7623a17f8 233 target_pos = 0;
arvindnr89 15:d6d7623a17f8 234 motor1 = 0;
arvindnr89 15:d6d7623a17f8 235 motor2 = 0;
arvindnr89 15:d6d7623a17f8 236 control_output = 0;
arvindnr89 15:d6d7623a17f8 237 fallen = false;
arvindnr89 15:d6d7623a17f8 238 }
csharer 18:ee252b8f7430 239
arvindnr89 11:2553f5798f84 240 //Get the time stamp as soon as it enters the loop
arvindnr89 11:2553f5798f84 241 timer_value = timer.read_us();
arvindnr89 11:2553f5798f84 242 led4 = !fallen;
arvindnr89 11:2553f5798f84 243 led2 = button;
csharer 9:fb671fa0c0be 244
csharer 18:ee252b8f7430 245 //Set Gainz With Knobs
csharer 18:ee252b8f7430 246 Kp1 = ((float)knob1);
csharer 18:ee252b8f7430 247 Kd1 = ((float)knob2);
csharer 18:ee252b8f7430 248 Kp2 = ((float)knob3);
csharer 18:ee252b8f7430 249 Kd2 = ((float)knob4);
arvindnr89 11:2553f5798f84 250
csharer 18:ee252b8f7430 251 //Joystick Control
csharer 18:ee252b8f7430 252 throttle = (float)jstick_v / THROTTLE_DAMPNER;
csharer 18:ee252b8f7430 253 steering = (float)jstick_h / STREEING_DAMPNER;
arvindnr89 11:2553f5798f84 254
arvindnr89 11:2553f5798f84 255 //Calculate the delta time
arvindnr89 11:2553f5798f84 256 dt = (timer_value - timer_old);
arvindnr89 11:2553f5798f84 257 timer_old = timer_value;
csharer 19:19d72dc64b43 258 //angle_old = angle;
arvindnr89 17:8e2824f64b91 259
arvindnr89 11:2553f5798f84 260 // STANDING: Motor Control Enabled
arvindnr89 11:2553f5798f84 261 //******************** PID Control BEGIN ********************** //
arvindnr89 11:2553f5798f84 262 if(((angle < 45) && (angle > -45)) && (fallen == false)) {
arvindnr89 11:2553f5798f84 263
arvindnr89 11:2553f5798f84 264 //Robot Position
arvindnr89 11:2553f5798f84 265 robot_pos = (pos_M1 + pos_M2) / 2;
csharer 19:19d72dc64b43 266
csharer 19:19d72dc64b43 267 //Target Position Incremented with Joystick
csharer 19:19d72dc64b43 268 target_pos += throttle;
arvindnr89 12:16d1a5390022 269
csharer 18:ee252b8f7430 270 //*************** Velocity Computation **********************
arvindnr89 12:16d1a5390022 271 velocitySamplesCounter++;
arvindnr89 12:16d1a5390022 272 if(velocitySamplesCounter == VELOCITY_SAMPLES){
arvindnr89 12:16d1a5390022 273 velocitySamplesCounter = 0;
arvindnr89 12:16d1a5390022 274 }
arvindnr89 12:16d1a5390022 275 runningSumReplaceVal = velocitySamples[velocitySamplesCounter]; //value to replace
arvindnr89 12:16d1a5390022 276 newVelocity = (motor1 + motor2) / 2;
arvindnr89 12:16d1a5390022 277 velocitySamples[velocitySamplesCounter] = newVelocity; //replace value with
arvindnr89 12:16d1a5390022 278 velocitySum = velocitySum - runningSumReplaceVal + newVelocity;
arvindnr89 12:16d1a5390022 279 velocity = velocitySum/VELOCITY_SAMPLES;
csharer 14:94c65d1c8bad 280
csharer 20:a7cba632d0b1 281 /*##################################################################
csharer 20:a7cba632d0b1 282 ########## ALL CODING FOR THIS LAB WILL BE COMPLETED BELOW #########
csharer 20:a7cba632d0b1 283 ######### THERE ARE 2 PARTS, PART 1 MUST BE COMPLETED FIRST ########
csharer 20:a7cba632d0b1 284 ####### IT IS NOT RECOMENDED TO MODIFY CODE OUTSIDE THIS LOOP ######
csharer 20:a7cba632d0b1 285 ##################################################################*/
csharer 20:a7cba632d0b1 286
csharer 20:a7cba632d0b1 287 //PART 2
csharer 19:19d72dc64b43 288 // ***************** Position Controller *********************
csharer 19:19d72dc64b43 289 //Inputs: robot_position, target_pos
csharer 19:19d72dc64b43 290 //Error: distance from target position
csharer 19:19d72dc64b43 291 //Output: target_angle
arvindnr89 15:d6d7623a17f8 292
csharer 18:ee252b8f7430 293 float Kp1P = Kp2/1000;
csharer 18:ee252b8f7430 294 float Kd1P = Kd2/100;
csharer 19:19d72dc64b43 295
csharer 20:a7cba632d0b1 296 //(2.0) Calculate the Position Error using robot_pos and target_pos
csharer 20:a7cba632d0b1 297 pos_error = 0;
arvindnr89 11:2553f5798f84 298
csharer 20:a7cba632d0b1 299 //(2.1) Calculate the Proportional Term (P Term) Using The Error
csharer 20:a7cba632d0b1 300 kp_pos_term = 0;
arvindnr89 11:2553f5798f84 301
csharer 20:a7cba632d0b1 302 //(2.2) Calculate the Derivative Term (D Term) Using The Error
csharer 20:a7cba632d0b1 303 change_in_target_pos = 0;
csharer 20:a7cba632d0b1 304 kd_pos_term = 0; //Use change_in_target_pos and velocity
csharer 20:a7cba632d0b1 305 pos_control_output = 0; //Add the two terms together (P + D)
csharer 20:a7cba632d0b1 306 pos_control_output = CAP(pos_control_output, 100); //Cap the values
csharer 20:a7cba632d0b1 307
csharer 20:a7cba632d0b1 308 //This Control Output Will Feed Into The Stability Controller
csharer 20:a7cba632d0b1 309 target_angle = -pos_control_output;
csharer 19:19d72dc64b43 310
csharer 20:a7cba632d0b1 311 //PART 1 (Do this before the Position Controller)
csharer 19:19d72dc64b43 312 //************ PD Stability CONTROLLER HERE ****************
csharer 19:19d72dc64b43 313 //Inputs: angle, target_angle
csharer 19:19d72dc64b43 314 //Error: distance from target_angle
csharer 19:19d72dc64b43 315 //Output: control_output (motor speed)
csharer 18:ee252b8f7430 316
csharer 19:19d72dc64b43 317 //Scale the Knob Values;
csharer 19:19d72dc64b43 318 float Kp1S = Kp1/100;
csharer 19:19d72dc64b43 319 float Kd1S = Kd1*100;
arvindnr89 12:16d1a5390022 320
csharer 20:a7cba632d0b1 321 //(1.0) Calculate the Angle Error Using target_angle and angle
csharer 20:a7cba632d0b1 322 error = 0;
csharer 18:ee252b8f7430 323
csharer 20:a7cba632d0b1 324 //(1.1) Calculate the Proportional Term (P term) Using the Error
csharer 20:a7cba632d0b1 325 kp_term = 0;
arvindnr89 11:2553f5798f84 326
csharer 20:a7cba632d0b1 327 //(1.2) Calculate the Derivatve Term (D term)
csharer 20:a7cba632d0b1 328 change_in_target_angle = 0;
csharer 20:a7cba632d0b1 329 change_in_angle = 0; //Look 2 samples back (use angle_old2)
csharer 20:a7cba632d0b1 330 kd_term = 0;
arvindnr89 11:2553f5798f84 331
csharer 20:a7cba632d0b1 332 //(1.3) Calculate the Control Output
csharer 20:a7cba632d0b1 333 control_output += 0; //Add the P and D terms together here
csharer 18:ee252b8f7430 334 control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
csharer 18:ee252b8f7430 335
csharer 18:ee252b8f7430 336 //*************** Set Motor Speed *************************
csharer 19:19d72dc64b43 337 motor1 = (int16_t)(control_output + (steering));
csharer 19:19d72dc64b43 338 motor2 = (int16_t)(control_output - (steering));
arvindnr89 11:2553f5798f84 339 motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
arvindnr89 11:2553f5798f84 340 motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
csharer 18:ee252b8f7430 341 setMotor1Speed(-motor1);
csharer 18:ee252b8f7430 342 setMotor2Speed(-motor2);
arvindnr89 11:2553f5798f84 343
arvindnr89 11:2553f5798f84 344 //Update variables
arvindnr89 11:2553f5798f84 345 target_angle_old = target_angle;
arvindnr89 11:2553f5798f84 346 angle_old2 = angle_old1;
arvindnr89 11:2553f5798f84 347 angle_old1 = angle;
arvindnr89 11:2553f5798f84 348
csharer 18:ee252b8f7430 349 //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 18:ee252b8f7430 350
csharer 13:a18d41a60d0b 351 //Enable Motors
arvindnr89 11:2553f5798f84 352 enable = ENABLE;
arvindnr89 16:3a85662fb740 353
arvindnr89 16:3a85662fb740 354 if(abs(motor1) == MAX_CONTROL_OUTPUT || abs(motor2) == MAX_CONTROL_OUTPUT) {
arvindnr89 17:8e2824f64b91 355 pc.printf("Max Speed Reached. Killing the Robot\n");
arvindnr89 17:8e2824f64b91 356 enable = DISABLE;
arvindnr89 16:3a85662fb740 357 fallen = true;
arvindnr89 16:3a85662fb740 358 }
arvindnr89 11:2553f5798f84 359 } else { //[FALLEN}
arvindnr89 11:2553f5798f84 360 //Disable Motors
arvindnr89 11:2553f5798f84 361 enable = DISABLE;
arvindnr89 11:2553f5798f84 362
arvindnr89 11:2553f5798f84 363 //Set fallen flag
arvindnr89 11:2553f5798f84 364 fallen = true;
arvindnr89 11:2553f5798f84 365 }
csharer 18:ee252b8f7430 366 osSignalClear(pid_update_thread_ID,PID_UPDATE_SIGNAL);
arvindnr89 11:2553f5798f84 367 } //end main loop
arvindnr89 11:2553f5798f84 368 }
arvindnr89 11:2553f5798f84 369
arvindnr89 11:2553f5798f84 370 // ================================================================
arvindnr89 11:2553f5798f84 371 // === Communication Thread ===
arvindnr89 11:2553f5798f84 372 // ================================================================
arvindnr89 11:2553f5798f84 373 void communication_update_thread(void const *args)
arvindnr89 11:2553f5798f84 374 {
arvindnr89 11:2553f5798f84 375 pc.printf("Starting Communication Thread..\r\n");
arvindnr89 11:2553f5798f84 376 while(1) {
arvindnr89 11:2553f5798f84 377 //Recieve Data
arvindnr89 11:2553f5798f84 378 rxLen = rf_receive(rxBuffer, 128);
arvindnr89 11:2553f5798f84 379 if(rxLen > 0) {
arvindnr89 11:2553f5798f84 380 led1 = led1^1;
arvindnr89 11:2553f5798f84 381 //Process data with our protocal
arvindnr89 11:2553f5798f84 382 communication_protocal(rxLen);
arvindnr89 11:2553f5798f84 383 }
arvindnr89 11:2553f5798f84 384 }
arvindnr89 11:2553f5798f84 385 }
arvindnr89 10:48f640a54401 386
csharer 3:2f76ffbc5cef 387 // ================================================================
csharer 3:2f76ffbc5cef 388 // === INITIAL SETUP ===
csharer 3:2f76ffbc5cef 389 // ================================================================
csharer 3:2f76ffbc5cef 390 void init_imu()
csharer 3:2f76ffbc5cef 391 {
arvindnr89 10:48f640a54401 392 pc.printf("Start IMU Initilization.. \r\n");
csharer 3:2f76ffbc5cef 393
csharer 3:2f76ffbc5cef 394 // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
csharer 3:2f76ffbc5cef 395 mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
csharer 3:2f76ffbc5cef 396 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
csharer 3:2f76ffbc5cef 397 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
csharer 3:2f76ffbc5cef 398 mpu.setDLPFMode(MPU6050_DLPF_BW_10); //10,20,42,98,188 // Default factor for BROBOT:10
arvindnr89 17:8e2824f64b91 399 mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz [4=200hz]default
csharer 3:2f76ffbc5cef 400 mpu.setSleepEnabled(false);
csharer 3:2f76ffbc5cef 401 wait_ms(500);
csharer 3:2f76ffbc5cef 402
csharer 3:2f76ffbc5cef 403 // load and configure the DMP
csharer 3:2f76ffbc5cef 404 devStatus = mpu.dmpInitialize();
csharer 3:2f76ffbc5cef 405 if(devStatus == 0) {
csharer 3:2f76ffbc5cef 406 mpu.setDMPEnabled(true);
csharer 3:2f76ffbc5cef 407 mpuIntStatus = mpu.getIntStatus();
csharer 3:2f76ffbc5cef 408 dmpReady = true;
csharer 4:2512939c10f0 409 } else {
csharer 3:2f76ffbc5cef 410 // 1 = initial memory load failed
csharer 3:2f76ffbc5cef 411 // 2 = DMP configuration updates failed
csharer 3:2f76ffbc5cef 412 pc.printf("DMP INIT error \r\n");
csharer 3:2f76ffbc5cef 413 }
csharer 3:2f76ffbc5cef 414
csharer 3:2f76ffbc5cef 415 //Gyro Calibration
csharer 3:2f76ffbc5cef 416 wait_ms(500);
arvindnr89 10:48f640a54401 417 pc.printf("Gyro calibration!! Dont move the robot in 1 second... \r\n");
csharer 3:2f76ffbc5cef 418 wait_ms(500);
csharer 4:2512939c10f0 419
csharer 3:2f76ffbc5cef 420 // verify connection
csharer 3:2f76ffbc5cef 421 pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");
csharer 3:2f76ffbc5cef 422
csharer 3:2f76ffbc5cef 423 //Adjust Sensor Fusion Gain
csharer 3:2f76ffbc5cef 424 dmpSetSensorFusionAccelGain(0x20);
csharer 3:2f76ffbc5cef 425
csharer 3:2f76ffbc5cef 426 wait_ms(200);
csharer 3:2f76ffbc5cef 427 mpu.resetFIFO();
csharer 3:2f76ffbc5cef 428 }
csharer 3:2f76ffbc5cef 429
arvindnr89 10:48f640a54401 430 void init_system()
arvindnr89 10:48f640a54401 431 {
arvindnr89 10:48f640a54401 432 initilizeAngle = true;
arvindnr89 17:8e2824f64b91 433 initilizeTheta = true;
arvindnr89 16:3a85662fb740 434 totalTheta = 0;
arvindnr89 16:3a85662fb740 435 target_pos = 0;
arvindnr89 16:3a85662fb740 436 pos_error = 0;
arvindnr89 10:48f640a54401 437 //Set the Channel. 0 is default, 15 is max
arvindnr89 10:48f640a54401 438 mrf.SetChannel(MRF_CHANNEL);
arvindnr89 10:48f640a54401 439 enable = DISABLE; //Disable Motors
arvindnr89 10:48f640a54401 440 pc.baud(115200);
arvindnr89 10:48f640a54401 441 pc.printf("Booting BroBot mbed RTOS..\r\n");
arvindnr89 10:48f640a54401 442
arvindnr89 10:48f640a54401 443 //Initilize the IMU
arvindnr89 10:48f640a54401 444 init_imu();
arvindnr89 10:48f640a54401 445 //Attach Interupt for IMU on rising edge of checkpin
arvindnr89 10:48f640a54401 446 checkpin.rise(&dmpDataReady);
arvindnr89 10:48f640a54401 447 pc.printf("IMU Initilized..\r\n");
arvindnr89 10:48f640a54401 448
arvindnr89 10:48f640a54401 449 //Init Stepper Motors
arvindnr89 10:48f640a54401 450 //Attach Motor Control Timer Call Back Functions
arvindnr89 10:48f640a54401 451 timer_M1.attach_us(&ISR1, ZERO_SPEED);//1s Period
arvindnr89 10:48f640a54401 452 timer_M2.attach_us(&ISR2, ZERO_SPEED);//1s Period
arvindnr89 10:48f640a54401 453 step_M1 = 1;
arvindnr89 10:48f640a54401 454 dir_M1 = 1;
arvindnr89 10:48f640a54401 455 pc.printf("Motors Initilized..\r\n");
arvindnr89 10:48f640a54401 456
arvindnr89 10:48f640a54401 457 //Timers initilized
arvindnr89 10:48f640a54401 458 timer.start();
arvindnr89 10:48f640a54401 459 timer_value = timer.read_us();
arvindnr89 10:48f640a54401 460
arvindnr89 11:2553f5798f84 461 enable = ENABLE; //Enable Motors
arvindnr89 10:48f640a54401 462 }
arvindnr89 10:48f640a54401 463
csharer 3:2f76ffbc5cef 464 // ================================================================
csharer 3:2f76ffbc5cef 465 // === MAIN PROGRAM LOOP ===
csharer 3:2f76ffbc5cef 466 // ================================================================
csharer 3:2f76ffbc5cef 467 int main()
csharer 3:2f76ffbc5cef 468 {
arvindnr89 10:48f640a54401 469 init_system();
csharer 4:2512939c10f0 470
arvindnr89 10:48f640a54401 471 //Create IMU Thread
arvindnr89 11:2553f5798f84 472 imu_update_thread_ID = osThreadCreate(osThread(imu_update_thread), NULL);
csharer 9:fb671fa0c0be 473
arvindnr89 11:2553f5798f84 474 //Create PID Thread
arvindnr89 11:2553f5798f84 475 pid_update_thread_ID = osThreadCreate(osThread(pid_update_thread), NULL);
csharer 9:fb671fa0c0be 476
arvindnr89 11:2553f5798f84 477 //Create Communication Thread
arvindnr89 11:2553f5798f84 478 communication_update_thread_ID = osThreadCreate(osThread(communication_update_thread), NULL);
csharer 4:2512939c10f0 479
csharer 3:2f76ffbc5cef 480 } //End Main()