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@16:3a85662fb740, 2017-02-08 (annotated)

Committer:
arvindnr89
Date:
Wed Feb 08 22:33:21 2017 +0000
Revision:
16:3a85662fb740
Parent:
15:d6d7623a17f8
Child:
17:8e2824f64b91
started trajectory folowing

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