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@10:48f640a54401, 2017-01-10 (annotated)

Committer:
arvindnr89
Date:
Tue Jan 10 21:10:39 2017 +0000
Revision:
10:48f640a54401
Parent:
9:fb671fa0c0be
Child:
11:2553f5798f84
Cleaned up code and improved efficiency

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