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@15:d6d7623a17f8, 2017-01-26 (annotated)

Committer:
arvindnr89
Date:
Thu Jan 26 21:37:24 2017 +0000
Revision:
15:d6d7623a17f8
Parent:
14:94c65d1c8bad
Child:
16:3a85662fb740
Introduced theta controller and added some debug statements to the imu handling

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