Carter Sharer / Mbed 2 deprecated BroBot_RTOS_ESE350

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU6050_V3 mbed-rtos mbed

Fork of BroBot_RTOS_Development by Arvind Ramesh

main.cpp@18:ee252b8f7430, 2017-03-22 (annotated)

Committer:
csharer
Date:
Wed Mar 22 21:01:07 2017 +0000
Revision:
18:ee252b8f7430
Parent:
17:8e2824f64b91
Child:
19:19d72dc64b43
Balance Bot Code Cleaned for Lab handout

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