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@11:2553f5798f84, 2017-01-19 (annotated)

Committer:
arvindnr89
Date:
Thu Jan 19 19:18:54 2017 +0000
Revision:
11:2553f5798f84
Parent:
10:48f640a54401
Child:
12:16d1a5390022
Working code with bug fixes

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