Flex robot / Mbed 2 deprecated FlexV1

Important changes to repositories hosted on mbed.com

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

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

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

Dependencies:   mbed Servo MC33926 encoder

Fork of FlexV1 by Ferréol GAGEY

main.cpp@8:3c174bef1dbd, 2021-03-27 (annotated)

Committer:
fgagey
Date:
Sat Mar 27 00:06:06 2021 +0000
Revision:
8:3c174bef1dbd
Parent:
7:fe796ee2f59f
Child:
9:7b0f9fd4586b
Flex balancing robot ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fgagey 8:3c174bef1dbd 1 // Flex balancing robot using MPU6050 DMP (MotionApps v2.0)
fgagey 8:3c174bef1dbd 2 // 26/03/2021 by Ferréol Gagey <ferreol.gagey@ens-paris-saclay.fr>
rolfz 6:029b4b8111ef 3
fgagey 8:3c174bef1dbd 4 // Changelog:
fgagey 8:3c174bef1dbd 5 // 26/03/2021 - simple pid without encoder
rolfz 6:029b4b8111ef 6
fgagey 8:3c174bef1dbd 7 //===============================================
rolfz 6:029b4b8111ef 8
rolfz 6:029b4b8111ef 9
rolfz 6:029b4b8111ef 10 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
rolfz 6:029b4b8111ef 11 // for both classes must be in the include path of your project
rolfz 6:029b4b8111ef 12 #include "I2Cdev.h"
rolfz 6:029b4b8111ef 13 #include "mbed.h"
rolfz 6:029b4b8111ef 14 #include <math.h>
fgagey 8:3c174bef1dbd 15 #include "MC33926.h"
fgagey 8:3c174bef1dbd 16 #include "QEI.h"
fgagey 8:3c174bef1dbd 17 //DigitalOut leds[] = {(LED1), (LED2),(LED3),(LED4)};
rolfz 6:029b4b8111ef 18
rolfz 6:029b4b8111ef 19
rolfz 6:029b4b8111ef 20 #include "MPU6050_6Axis_MotionApps20.h" // works
rolfz 6:029b4b8111ef 21
rolfz 6:029b4b8111ef 22 MPU6050 mpu;
rolfz 6:029b4b8111ef 23
rolfz 6:029b4b8111ef 24
rolfz 6:029b4b8111ef 25 #ifndef M_PI
rolfz 6:029b4b8111ef 26 #define M_PI 3.1415
rolfz 6:029b4b8111ef 27 #endif
rolfz 6:029b4b8111ef 28
rolfz 6:029b4b8111ef 29
rolfz 6:029b4b8111ef 30 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
rolfz 6:029b4b8111ef 31 // pitch/roll angles (in degrees) calculated from the quaternions coming
rolfz 6:029b4b8111ef 32 // from the FIFO. Note this also requires gravity vector calculations.
rolfz 6:029b4b8111ef 33 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
rolfz 6:029b4b8111ef 34 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
fgagey 8:3c174bef1dbd 35 #define OUTPUT_READABLE_YAWPITCHROLL
rolfz 6:029b4b8111ef 36
rolfz 6:029b4b8111ef 37
rolfz 6:029b4b8111ef 38 bool blinkState = false;
rolfz 6:029b4b8111ef 39
rolfz 6:029b4b8111ef 40 // MPU control/status vars
rolfz 6:029b4b8111ef 41 bool dmpReady = false; // set true if DMP init was successful
rolfz 6:029b4b8111ef 42 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
rolfz 6:029b4b8111ef 43 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
rolfz 6:029b4b8111ef 44 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
rolfz 6:029b4b8111ef 45 uint16_t fifoCount; // count of all bytes currently in FIFO
rolfz 6:029b4b8111ef 46 uint8_t fifoBuffer[64]; // FIFO storage buffer
rolfz 6:029b4b8111ef 47
rolfz 6:029b4b8111ef 48 // orientation/motion vars
rolfz 6:029b4b8111ef 49 Quaternion q; // [w, x, y, z] quaternion container
rolfz 6:029b4b8111ef 50 VectorInt16 aa; // [x, y, z] accel sensor measurements
rolfz 6:029b4b8111ef 51 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
rolfz 6:029b4b8111ef 52 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
rolfz 6:029b4b8111ef 53 VectorFloat gravity; // [x, y, z] gravity vector
rolfz 6:029b4b8111ef 54 float euler[3]; // [psi, theta, phi] Euler angle container
rolfz 6:029b4b8111ef 55 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
rolfz 6:029b4b8111ef 56
rolfz 6:029b4b8111ef 57 // packet structure for InvenSense teapot demo
rolfz 6:029b4b8111ef 58 uint8_t teapotPacket[15] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n',0 };
rolfz 6:029b4b8111ef 59
rolfz 6:029b4b8111ef 60 // ================================================================
rolfz 6:029b4b8111ef 61 // === INTERRUPT DETECTION ROUTINE ===
rolfz 6:029b4b8111ef 62 // ================================================================
rolfz 6:029b4b8111ef 63
rolfz 6:029b4b8111ef 64 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
rolfz 6:029b4b8111ef 65 void dmpDataReady() {
rolfz 6:029b4b8111ef 66 mpuInterrupt = true;
rolfz 6:029b4b8111ef 67 }
rolfz 6:029b4b8111ef 68
fgagey 8:3c174bef1dbd 69 /* Motors*/
fgagey 8:3c174bef1dbd 70 //MC33926 motorRight(D8,D9,A2);
fgagey 8:3c174bef1dbd 71 MC33926 motorLeft(D4,D6,A3);
fgagey 8:3c174bef1dbd 72
fgagey 8:3c174bef1dbd 73 /* Encoders*/
fgagey 8:3c174bef1dbd 74
fgagey 8:3c174bef1dbd 75 QEI rightEncoder(D3,D2,NC,360);
fgagey 8:3c174bef1dbd 76 QEI leftEncoder(A4,A5,NC,360);
fgagey 8:3c174bef1dbd 77 int pulseleftcoder;
fgagey 8:3c174bef1dbd 78 int pulserightcoder;
fgagey 8:3c174bef1dbd 79
fgagey 8:3c174bef1dbd 80 /* bluetooth module*/
fgagey 8:3c174bef1dbd 81 Serial blue(D1, D0); // Bluetooth TX, RX
fgagey 8:3c174bef1dbd 82
fgagey 8:3c174bef1dbd 83
fgagey 8:3c174bef1dbd 84 /* Variable declarations */
fgagey 8:3c174bef1dbd 85 float pitchAngle = 0;
fgagey 8:3c174bef1dbd 86 float rollAngle = 0;
fgagey 8:3c174bef1dbd 87 bool dir; // direction of movement
fgagey 8:3c174bef1dbd 88 float Kp = 0.5;
fgagey 8:3c174bef1dbd 89 float Ki = 0.00001;
fgagey 8:3c174bef1dbd 90 float Kd = 0.01;//0.01;//0.05;
fgagey 8:3c174bef1dbd 91 float set_point = -0.8; // Angle for staying upright
fgagey 8:3c174bef1dbd 92 float errorPID = 0; // Proportional term (P) in PID
fgagey 8:3c174bef1dbd 93 float last_errorPID =0; // Previous error value
fgagey 8:3c174bef1dbd 94 float integral = 0; // Integral (I)
fgagey 8:3c174bef1dbd 95 float derivative = 0; // Derivative (D)
fgagey 8:3c174bef1dbd 96 float outputPID = 0; // Output of PID calculations
fgagey 8:3c174bef1dbd 97 int phone_char; //char returned by th bluetooth module
fgagey 8:3c174bef1dbd 98
fgagey 8:3c174bef1dbd 99
fgagey 8:3c174bef1dbd 100
fgagey 8:3c174bef1dbd 101 Ticker toggler1; // Ticker for led toggling
fgagey 8:3c174bef1dbd 102 Ticker gyro; // Ticker for periodic call to compFilter funcçs
fgagey 8:3c174bef1dbd 103 Ticker balance; // Periodic routine for PID control of balance system
fgagey 8:3c174bef1dbd 104 Ticker speed; // Periodic routine for speed control
fgagey 8:3c174bef1dbd 105 Ticker bluetooth; // Ticker for navigation
fgagey 8:3c174bef1dbd 106
fgagey 8:3c174bef1dbd 107 void toggle_led1();
fgagey 8:3c174bef1dbd 108 void toggle_led2();
fgagey 8:3c174bef1dbd 109 void getAngle();
fgagey 8:3c174bef1dbd 110 void balancePID();
fgagey 8:3c174bef1dbd 111 void balanceControl();
fgagey 8:3c174bef1dbd 112 void Forward(float);
fgagey 8:3c174bef1dbd 113 void Reverse(float);
fgagey 8:3c174bef1dbd 114 void Stop(void);
fgagey 8:3c174bef1dbd 115 void Navigate();
fgagey 8:3c174bef1dbd 116 void TurnRight(float);
fgagey 8:3c174bef1dbd 117 void TurnLeft(float);
fgagey 8:3c174bef1dbd 118
rolfz 6:029b4b8111ef 119 // ================================================================
rolfz 6:029b4b8111ef 120 // === INITIAL SETUP ===
rolfz 6:029b4b8111ef 121 // ================================================================
rolfz 6:029b4b8111ef 122
rolfz 6:029b4b8111ef 123 int main()
rolfz 6:029b4b8111ef 124 {
rolfz 6:029b4b8111ef 125
rolfz 6:029b4b8111ef 126 //Pin Defines for I2C Bus
fgagey 8:3c174bef1dbd 127 #define D_SDA D14
fgagey 8:3c174bef1dbd 128 #define D_SCL D15
rolfz 6:029b4b8111ef 129 //#define D_SDA p28
rolfz 6:029b4b8111ef 130 //#define D_SCL p27
rolfz 6:029b4b8111ef 131 I2C i2c(D_SDA, D_SCL);
rolfz 6:029b4b8111ef 132
rolfz 6:029b4b8111ef 133
rolfz 6:029b4b8111ef 134 // mbed Interface Hardware definitions
fgagey 8:3c174bef1dbd 135 //DigitalOut myled1(LED1);
fgagey 8:3c174bef1dbd 136 //DigitalOut myled2(LED2);
fgagey 8:3c174bef1dbd 137 //DigitalOut myled3(LED3);
fgagey 8:3c174bef1dbd 138 //DigitalOut heartbeatLED(LED4);
rolfz 6:029b4b8111ef 139
rolfz 6:029b4b8111ef 140 // initialize serial communication
rolfz 6:029b4b8111ef 141 // (115200 chosen because it is required for Teapot Demo output, but it's
rolfz 6:029b4b8111ef 142 // really up to you depending on your project)
rolfz 6:029b4b8111ef 143 //Host PC Baudrate (Virtual Com Port on USB)
rolfz 6:029b4b8111ef 144 #define D_BAUDRATE 115200
rolfz 6:029b4b8111ef 145
rolfz 6:029b4b8111ef 146 // Host PC Communication channels
rolfz 6:029b4b8111ef 147 Serial pc(USBTX, USBRX); // tx, rx
rolfz 6:029b4b8111ef 148
rolfz 6:029b4b8111ef 149 pc.baud(D_BAUDRATE);
rolfz 6:029b4b8111ef 150 // initialize device
rolfz 6:029b4b8111ef 151 pc.printf("Initializing I2C devices...\n");
rolfz 6:029b4b8111ef 152 mpu.initialize();
rolfz 6:029b4b8111ef 153
rolfz 6:029b4b8111ef 154 // verify connection
rolfz 6:029b4b8111ef 155 pc.printf("Testing device connections...\n");
rolfz 6:029b4b8111ef 156
rolfz 6:029b4b8111ef 157 bool mpu6050TestResult = mpu.testConnection();
rolfz 6:029b4b8111ef 158 if(mpu6050TestResult){
rolfz 6:029b4b8111ef 159 pc.printf("MPU6050 test passed \n");
rolfz 6:029b4b8111ef 160 } else{
rolfz 6:029b4b8111ef 161 pc.printf("MPU6050 test failed \n");
rolfz 6:029b4b8111ef 162 }
rolfz 6:029b4b8111ef 163
rolfz 6:029b4b8111ef 164 // wait for ready
rolfz 6:029b4b8111ef 165 pc.printf("\nSend any character to begin DMP programming and demo: ");
rolfz 6:029b4b8111ef 166
fgagey 8:3c174bef1dbd 167 //while(!pc.readable());
fgagey 8:3c174bef1dbd 168 //pc.getc();
fgagey 8:3c174bef1dbd 169 //pc.printf("\n");
rolfz 6:029b4b8111ef 170
rolfz 6:029b4b8111ef 171 // load and configure the DMP
rolfz 6:029b4b8111ef 172 pc.printf("Initializing DMP...\n");
rolfz 6:029b4b8111ef 173 devStatus = mpu.dmpInitialize();
rolfz 6:029b4b8111ef 174
rolfz 6:029b4b8111ef 175 // supply your own gyro offsets here, scaled for min sensitivity
fgagey 8:3c174bef1dbd 176 mpu.setXGyroOffset(62);
fgagey 8:3c174bef1dbd 177 mpu.setYGyroOffset(1);
fgagey 8:3c174bef1dbd 178 mpu.setZGyroOffset(63);
rolfz 6:029b4b8111ef 179 mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
rolfz 6:029b4b8111ef 180
rolfz 6:029b4b8111ef 181 // make sure it worked (returns 0 if so)
rolfz 6:029b4b8111ef 182 if (devStatus == 0) {
rolfz 6:029b4b8111ef 183 // turn on the DMP, now that it's ready
rolfz 6:029b4b8111ef 184 pc.printf("Enabling DMP...\n");
rolfz 6:029b4b8111ef 185 mpu.setDMPEnabled(true);
rolfz 6:029b4b8111ef 186
rolfz 6:029b4b8111ef 187 // enable Arduino interrupt detection
rolfz 6:029b4b8111ef 188 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n");
rolfz 6:029b4b8111ef 189 // attachInterrupt(0, dmpDataReady, RISING);
rolfz 6:029b4b8111ef 190 mpuIntStatus = mpu.getIntStatus();
rolfz 6:029b4b8111ef 191
rolfz 6:029b4b8111ef 192 // set our DMP Ready flag so the main loop() function knows it's okay to use it
rolfz 6:029b4b8111ef 193 pc.printf("DMP ready! Waiting for first interrupt...\n");
rolfz 6:029b4b8111ef 194 dmpReady = true;
rolfz 6:029b4b8111ef 195
rolfz 6:029b4b8111ef 196 // get expected DMP packet size for later comparison
rolfz 6:029b4b8111ef 197 packetSize = mpu.dmpGetFIFOPacketSize();
rolfz 6:029b4b8111ef 198 } else {
rolfz 6:029b4b8111ef 199 // ERROR!
rolfz 6:029b4b8111ef 200 // 1 = initial memory load failed
rolfz 6:029b4b8111ef 201 // 2 = DMP configuration updates failed
rolfz 6:029b4b8111ef 202 // (if it's going to break, usually the code will be 1)
rolfz 6:029b4b8111ef 203 pc.printf("DMP Initialization failed (code ");
rolfz 6:029b4b8111ef 204 pc.printf("%u",devStatus);
rolfz 6:029b4b8111ef 205 pc.printf(")\n");
rolfz 6:029b4b8111ef 206 }
fgagey 8:3c174bef1dbd 207 //gyro.attach(&getAngle, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
fgagey 8:3c174bef1dbd 208 balance.attach(&balancePID, 0.010); // Same period with balancePID
fgagey 8:3c174bef1dbd 209 speed.attach(&balanceControl, 0.01);
fgagey 8:3c174bef1dbd 210 bluetooth.attach(&Navigate, 0.05);
rolfz 6:029b4b8111ef 211
rolfz 6:029b4b8111ef 212
rolfz 6:029b4b8111ef 213 // ================================================================
rolfz 6:029b4b8111ef 214 // === MAIN PROGRAM LOOP ===
rolfz 6:029b4b8111ef 215 // ================================================================
rolfz 6:029b4b8111ef 216
rolfz 6:029b4b8111ef 217 while(1)
rolfz 6:029b4b8111ef 218 {
fgagey 8:3c174bef1dbd 219 if(blue.readable())
fgagey 8:3c174bef1dbd 220 {
fgagey 8:3c174bef1dbd 221 phone_char = blue.getc();
fgagey 8:3c174bef1dbd 222 pc.putc(phone_char);
fgagey 8:3c174bef1dbd 223 pc.printf("Bluetooth Start\r\n");
fgagey 8:3c174bef1dbd 224 //myled = !myled;
fgagey 8:3c174bef1dbd 225 }
fgagey 8:3c174bef1dbd 226 pulseleftcoder=leftEncoder.getPulses();
fgagey 8:3c174bef1dbd 227 pc.printf("compteur = %d ",pulseleftcoder);
fgagey 8:3c174bef1dbd 228 pc.printf("Roll Angle: %.1f, Pitch Angle: %.1f ",rollAngle,pitchAngle);
fgagey 8:3c174bef1dbd 229 pc.printf("Error = %f\r\n",outputPID);
fgagey 8:3c174bef1dbd 230
fgagey 8:3c174bef1dbd 231
rolfz 6:029b4b8111ef 232 // if programming failed, don't try to do anything
fgagey 8:3c174bef1dbd 233 if (!dmpReady) ;//continue;
fgagey 8:3c174bef1dbd 234 //myled2=0;
rolfz 6:029b4b8111ef 235
rolfz 6:029b4b8111ef 236 // wait for MPU interrupt or extra packet(s) available
rolfz 6:029b4b8111ef 237 // while (!mpuInterrupt && fifoCount < packetSize) {
rolfz 6:029b4b8111ef 238 // while (!mpuIntStatus && fifoCount < packetSize)
rolfz 6:029b4b8111ef 239 {
rolfz 6:029b4b8111ef 240 // other program behavior stuff here
rolfz 6:029b4b8111ef 241 // .
rolfz 6:029b4b8111ef 242 // .
rolfz 6:029b4b8111ef 243 // .
rolfz 6:029b4b8111ef 244 // if you are really paranoid you can frequently test in between other
rolfz 6:029b4b8111ef 245 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
rolfz 6:029b4b8111ef 246 // while() loop to immediately process the MPU data
rolfz 6:029b4b8111ef 247 // .
rolfz 6:029b4b8111ef 248 // .
rolfz 6:029b4b8111ef 249 // .
rolfz 6:029b4b8111ef 250 // fifoCount= mpu.getFIFOCount();
rolfz 6:029b4b8111ef 251 // mpuIntStatus = mpu.getIntStatus();
rolfz 6:029b4b8111ef 252 }
rolfz 6:029b4b8111ef 253 wait_us(500);
rolfz 6:029b4b8111ef 254
rolfz 6:029b4b8111ef 255 // reset interrupt flag and get INT_STATUS byte
rolfz 6:029b4b8111ef 256 mpuInterrupt = false;
rolfz 6:029b4b8111ef 257 mpuIntStatus = mpu.getIntStatus();
rolfz 6:029b4b8111ef 258
rolfz 6:029b4b8111ef 259 // get current FIFO count
rolfz 6:029b4b8111ef 260 fifoCount = mpu.getFIFOCount();
rolfz 6:029b4b8111ef 261
rolfz 6:029b4b8111ef 262 // check for overflow (this should never happen unless our code is too inefficient)
rolfz 6:029b4b8111ef 263 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
rolfz 6:029b4b8111ef 264 // reset so we can continue cleanly
rolfz 6:029b4b8111ef 265 mpu.resetFIFO();
fgagey 8:3c174bef1dbd 266 //pc.printf("FIFO overflow!");
rolfz 6:029b4b8111ef 267
rolfz 6:029b4b8111ef 268 // otherwise, check for DMP data ready interrupt (this should happen frequently)
rolfz 6:029b4b8111ef 269 } else if (mpuIntStatus & 0x02) {
rolfz 6:029b4b8111ef 270 // wait for correct available data length, should be a VERY short wait
rolfz 6:029b4b8111ef 271 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
rolfz 6:029b4b8111ef 272
rolfz 6:029b4b8111ef 273 // read a packet from FIFO
rolfz 6:029b4b8111ef 274 mpu.getFIFOBytes(fifoBuffer, packetSize);
rolfz 6:029b4b8111ef 275
rolfz 6:029b4b8111ef 276 // track FIFO count here in case there is > 1 packet available
rolfz 6:029b4b8111ef 277 // (this lets us immediately read more without waiting for an interrupt)
rolfz 6:029b4b8111ef 278 fifoCount -= packetSize;
rolfz 6:029b4b8111ef 279
rolfz 6:029b4b8111ef 280 #ifdef OUTPUT_READABLE_YAWPITCHROLL
rolfz 6:029b4b8111ef 281 // display Euler angles in degrees
rolfz 6:029b4b8111ef 282 mpu.dmpGetQuaternion(&q, fifoBuffer);
rolfz 6:029b4b8111ef 283 mpu.dmpGetGravity(&gravity, &q);
rolfz 6:029b4b8111ef 284 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
fgagey 8:3c174bef1dbd 285 rollAngle=ypr[1] * 180/M_PI;
fgagey 8:3c174bef1dbd 286 pitchAngle=ypr[2] * 180/M_PI;
fgagey 8:3c174bef1dbd 287 //pc.printf("ypr\t");
fgagey 8:3c174bef1dbd 288 // pc.printf("%f3.2",ypr[0] * 180/M_PI);
fgagey 8:3c174bef1dbd 289 // pc.printf("\t");
fgagey 8:3c174bef1dbd 290 // pc.printf("%f3.2",ypr[1] * 180/M_PI);
fgagey 8:3c174bef1dbd 291 // pc.printf("\t");
fgagey 8:3c174bef1dbd 292 // pc.printf("%f3.2\n",ypr[2] * 180/M_PI);
rolfz 6:029b4b8111ef 293 #endif
rolfz 6:029b4b8111ef 294 // blink LED to indicate activity
fgagey 8:3c174bef1dbd 295 //blinkState = !blinkState;
fgagey 8:3c174bef1dbd 296 //myled1 = blinkState;
rolfz 6:029b4b8111ef 297 }
rolfz 6:029b4b8111ef 298 }
fgagey 8:3c174bef1dbd 299 }
fgagey 8:3c174bef1dbd 300
fgagey 8:3c174bef1dbd 301 void getAngle(){
fgagey 8:3c174bef1dbd 302
fgagey 8:3c174bef1dbd 303 }
fgagey 8:3c174bef1dbd 304
fgagey 8:3c174bef1dbd 305 void balancePID()
fgagey 8:3c174bef1dbd 306 {
fgagey 8:3c174bef1dbd 307 errorPID = set_point - pitchAngle; //Proportional (P)
fgagey 8:3c174bef1dbd 308 integral += errorPID; //Integral (I)
fgagey 8:3c174bef1dbd 309 derivative = errorPID - last_errorPID; //Derivative (D)
fgagey 8:3c174bef1dbd 310
fgagey 8:3c174bef1dbd 311 last_errorPID = errorPID; //Save current value for next iteration
fgagey 8:3c174bef1dbd 312
fgagey 8:3c174bef1dbd 313 outputPID = (Kp * errorPID) + (Ki * integral) + (Kd * derivative); //PID calculation
fgagey 8:3c174bef1dbd 314
fgagey 8:3c174bef1dbd 315 /* errorPID is restricted between -1 and 1 */
fgagey 8:3c174bef1dbd 316 if(outputPID > 0.8)
fgagey 8:3c174bef1dbd 317 outputPID = 0.8;
fgagey 8:3c174bef1dbd 318 else if(outputPID < -0.8)
fgagey 8:3c174bef1dbd 319 outputPID = -0.8;
fgagey 8:3c174bef1dbd 320 }
fgagey 8:3c174bef1dbd 321
fgagey 8:3c174bef1dbd 322 void balanceControl()
fgagey 8:3c174bef1dbd 323 {
fgagey 8:3c174bef1dbd 324 int direction=0; // Variable to hold direction we want to drive
fgagey 8:3c174bef1dbd 325 if (outputPID>0)direction=1; // Positive speed indicates forward
fgagey 8:3c174bef1dbd 326 if (outputPID<0)direction=2; // Negative speed indicates backwards
fgagey 8:3c174bef1dbd 327 if((abs(pitchAngle)>10.00))direction=0;
fgagey 8:3c174bef1dbd 328
fgagey 8:3c174bef1dbd 329 switch( direction) { // Depending on what direction was passed
fgagey 8:3c174bef1dbd 330 case 0: // Stop case
fgagey 8:3c174bef1dbd 331 Stop();
fgagey 8:3c174bef1dbd 332 break;
fgagey 8:3c174bef1dbd 333 case 1: // Forward case
fgagey 8:3c174bef1dbd 334 Forward(abs(outputPID));
fgagey 8:3c174bef1dbd 335 break;
fgagey 8:3c174bef1dbd 336 case 2: // Backwards
fgagey 8:3c174bef1dbd 337 Reverse(abs(outputPID));
fgagey 8:3c174bef1dbd 338 break;
fgagey 8:3c174bef1dbd 339 default: // Catch-all (Stop)
fgagey 8:3c174bef1dbd 340 Stop();
fgagey 8:3c174bef1dbd 341 break;
fgagey 8:3c174bef1dbd 342 }
fgagey 8:3c174bef1dbd 343 }
fgagey 8:3c174bef1dbd 344
fgagey 8:3c174bef1dbd 345 void Stop(void)
fgagey 8:3c174bef1dbd 346 {
fgagey 8:3c174bef1dbd 347 //motorRight.SetPWMPulsewidth(2,0);
fgagey 8:3c174bef1dbd 348 motorLeft.SetPWMPulsewidth(2,0);
fgagey 8:3c174bef1dbd 349 }
fgagey 8:3c174bef1dbd 350
fgagey 8:3c174bef1dbd 351 void Forward(float s)
fgagey 8:3c174bef1dbd 352 {
fgagey 8:3c174bef1dbd 353 //motorRight.SetPWMPulsewidth(1,s);
fgagey 8:3c174bef1dbd 354 motorLeft.SetPWMPulsewidth(1,s);
fgagey 8:3c174bef1dbd 355 }
fgagey 8:3c174bef1dbd 356
fgagey 8:3c174bef1dbd 357 void TurnRight(float s)
fgagey 8:3c174bef1dbd 358 {
fgagey 8:3c174bef1dbd 359 //motorRight.SetPWMPulsewidth(0,s);
fgagey 8:3c174bef1dbd 360 motorLeft.SetPWMPulsewidth(1,s);
fgagey 8:3c174bef1dbd 361 }
fgagey 8:3c174bef1dbd 362
fgagey 8:3c174bef1dbd 363 void TurnLeft(float s)
fgagey 8:3c174bef1dbd 364 {
fgagey 8:3c174bef1dbd 365 //motorRight.SetPWMPulsewidth(1,s);
fgagey 8:3c174bef1dbd 366 motorLeft.SetPWMPulsewidth(0,s);
fgagey 8:3c174bef1dbd 367 }
fgagey 8:3c174bef1dbd 368
fgagey 8:3c174bef1dbd 369 void Reverse(float s)
fgagey 8:3c174bef1dbd 370 {
fgagey 8:3c174bef1dbd 371 //motorRight.SetPWMPulsewidth(0,s);
fgagey 8:3c174bef1dbd 372 motorLeft.SetPWMPulsewidth(0,s);
fgagey 8:3c174bef1dbd 373 }
fgagey 8:3c174bef1dbd 374
fgagey 8:3c174bef1dbd 375
fgagey 8:3c174bef1dbd 376
fgagey 8:3c174bef1dbd 377 void Navigate()
fgagey 8:3c174bef1dbd 378 {
fgagey 8:3c174bef1dbd 379 switch (phone_char)
fgagey 8:3c174bef1dbd 380 {
fgagey 8:3c174bef1dbd 381 case 'f':
fgagey 8:3c174bef1dbd 382 Forward(0.7);
fgagey 8:3c174bef1dbd 383 break;
fgagey 8:3c174bef1dbd 384
fgagey 8:3c174bef1dbd 385 case 'F':
fgagey 8:3c174bef1dbd 386 Forward(0.7);
fgagey 8:3c174bef1dbd 387 break;
fgagey 8:3c174bef1dbd 388
fgagey 8:3c174bef1dbd 389 case 'b':
fgagey 8:3c174bef1dbd 390 Reverse(0.7);
fgagey 8:3c174bef1dbd 391 break;
fgagey 8:3c174bef1dbd 392
fgagey 8:3c174bef1dbd 393 case 'B':
fgagey 8:3c174bef1dbd 394 Reverse(0.7);
fgagey 8:3c174bef1dbd 395 break;
fgagey 8:3c174bef1dbd 396
fgagey 8:3c174bef1dbd 397 case 'r':
fgagey 8:3c174bef1dbd 398 TurnRight(0.7);
fgagey 8:3c174bef1dbd 399 break;
fgagey 8:3c174bef1dbd 400
fgagey 8:3c174bef1dbd 401 case 'R':
fgagey 8:3c174bef1dbd 402 TurnRight(0.7);
fgagey 8:3c174bef1dbd 403 break;
fgagey 8:3c174bef1dbd 404
fgagey 8:3c174bef1dbd 405 case 'l':
fgagey 8:3c174bef1dbd 406 TurnLeft(0.7);
fgagey 8:3c174bef1dbd 407 break;
fgagey 8:3c174bef1dbd 408
fgagey 8:3c174bef1dbd 409 case 'L':
fgagey 8:3c174bef1dbd 410 TurnLeft(0.7);
fgagey 8:3c174bef1dbd 411 break;
fgagey 8:3c174bef1dbd 412
fgagey 8:3c174bef1dbd 413 default:
fgagey 8:3c174bef1dbd 414 Stop();
fgagey 8:3c174bef1dbd 415 }
fgagey 8:3c174bef1dbd 416 }