John Lee / Mbed 2 deprecated 4180Balancer

Balances a robot on two wheels using a PID controller for fine tuning the motor speed and behavior. It uses an IMU to send input to the PID controller and calibrate the robot's desired angle.

Dependencies:   mbed Motor LSM9DS1_Library_cal

main.cpp@3:8898bbbe00d6, 2019-12-06 (annotated)

Committer:
johnedwardlee
Date:
Fri Dec 06 13:47:56 2019 +0000
Revision:
3:8898bbbe00d6
Parent:
0:e8167f37725c 
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
johnedwardlee 3:8898bbbe00d6 1 /*
johnedwardlee 3:8898bbbe00d6 2 4180Balancer
johnedwardlee 3:8898bbbe00d6 3 Names: John Lee, Nyle Malik, Austin Lowe
johnedwardlee 3:8898bbbe00d6 4 Date: 12/6/19
johnedwardlee 3:8898bbbe00d6 5 Description:
johnedwardlee 3:8898bbbe00d6 6 Balances a robot on two wheels using a PID controller for fine tuning the
johnedwardlee 3:8898bbbe00d6 7 motor speed and behavior. It uses an IMU to send input to the PID controller
johnedwardlee 3:8898bbbe00d6 8 and calibrate the robot's desired angle.
johnedwardlee 3:8898bbbe00d6 9 */
johnedwardlee 3:8898bbbe00d6 10
jmar7 0:e8167f37725c 11 #include "LSM9DS1.h"
johnedwardlee 3:8898bbbe00d6 12 #include "Motor.h"
johnedwardlee 3:8898bbbe00d6 13 #include "mbed.h"
johnedwardlee 3:8898bbbe00d6 14 #include <time.h>
johnedwardlee 3:8898bbbe00d6 15 #include <stdlib.h>
johnedwardlee 3:8898bbbe00d6 16
johnedwardlee 3:8898bbbe00d6 17 #define RAD_TO_DEG 57.29578
johnedwardlee 3:8898bbbe00d6 18 #define M_PI 3.14159265358979323846
johnedwardlee 3:8898bbbe00d6 19 #define FILTER_CONSTANT 0.98
johnedwardlee 3:8898bbbe00d6 20 #define DT 0.001
jmar7 0:e8167f37725c 21
jmar7 0:e8167f37725c 22 DigitalOut myled(LED1);
johnedwardlee 3:8898bbbe00d6 23 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
johnedwardlee 3:8898bbbe00d6 24 Motor left(p21, p7, p8);
johnedwardlee 3:8898bbbe00d6 25 Motor right(p22, p11, p12);
johnedwardlee 3:8898bbbe00d6 26 Ticker PID;
jmar7 0:e8167f37725c 27 Serial pc(USBTX, USBRX);
jmar7 0:e8167f37725c 28
johnedwardlee 3:8898bbbe00d6 29 //Globals
johnedwardlee 3:8898bbbe00d6 30 //Configure these
johnedwardlee 3:8898bbbe00d6 31 float scalingFactor = 2000;
johnedwardlee 3:8898bbbe00d6 32 float KP = 100;
johnedwardlee 3:8898bbbe00d6 33 float KI = 100;
johnedwardlee 3:8898bbbe00d6 34 float KD = 1000;
johnedwardlee 3:8898bbbe00d6 35
johnedwardlee 3:8898bbbe00d6 36 float aBias = 0;
johnedwardlee 3:8898bbbe00d6 37 float gBias = 0;
johnedwardlee 3:8898bbbe00d6 38 float aAngle = 0;
johnedwardlee 3:8898bbbe00d6 39 float gAngle = 0;
johnedwardlee 3:8898bbbe00d6 40 float intAngle = 0;
johnedwardlee 3:8898bbbe00d6 41 float derivAngle = 0;
johnedwardlee 3:8898bbbe00d6 42 float propAngle = 0;
johnedwardlee 3:8898bbbe00d6 43 float angle = 0;
johnedwardlee 3:8898bbbe00d6 44 float speed = 0;
johnedwardlee 3:8898bbbe00d6 45
johnedwardlee 3:8898bbbe00d6 46 void calibrate()
johnedwardlee 3:8898bbbe00d6 47 {
johnedwardlee 3:8898bbbe00d6 48 for(int i=0; i<100; i++) {
johnedwardlee 3:8898bbbe00d6 49 imu.readAccel();
johnedwardlee 3:8898bbbe00d6 50 imu.readGyro();
johnedwardlee 3:8898bbbe00d6 51 aBias=aBias+(-1)*atan2((double)(imu.ay),(double)(imu.az))*180/3.142-90;
johnedwardlee 3:8898bbbe00d6 52 gBias=gBias+imu.gx;
johnedwardlee 3:8898bbbe00d6 53 }
johnedwardlee 3:8898bbbe00d6 54 aBias=aBias/100;
johnedwardlee 3:8898bbbe00d6 55 gBias=gBias/100;
johnedwardlee 3:8898bbbe00d6 56 }
johnedwardlee 3:8898bbbe00d6 57
johnedwardlee 3:8898bbbe00d6 58 void pid()
johnedwardlee 3:8898bbbe00d6 59 {
johnedwardlee 3:8898bbbe00d6 60 // Save the previous data
johnedwardlee 3:8898bbbe00d6 61 derivAngle = propAngle;
johnedwardlee 3:8898bbbe00d6 62
johnedwardlee 3:8898bbbe00d6 63 //Read data from the IMU
johnedwardlee 3:8898bbbe00d6 64 imu.readAccel();
johnedwardlee 3:8898bbbe00d6 65 imu.readGyro();
johnedwardlee 3:8898bbbe00d6 66
johnedwardlee 3:8898bbbe00d6 67 //Controls and PID calculations
johnedwardlee 3:8898bbbe00d6 68 aAngle = (-1) * atan2((double)(imu.ay), (double)(imu.az)) * RAD_TO_DEG - 90 - aBias;
johnedwardlee 3:8898bbbe00d6 69 gAngle = (-1)*(imu.gx - gBias) * DT;
johnedwardlee 3:8898bbbe00d6 70 propAngle = FILTER_CONSTANT * (propAngle + gAngle) + (1 - FILTER_CONSTANT) * aAngle - angle;
johnedwardlee 3:8898bbbe00d6 71 derivAngle = propAngle - derivAngle;
johnedwardlee 3:8898bbbe00d6 72 intAngle += (propAngle * DT); {
johnedwardlee 3:8898bbbe00d6 73 speed = -((KP * propAngle) + (KI * intAngle) + (KD * derivAngle)) / scalingFactor;
johnedwardlee 3:8898bbbe00d6 74 if (speed < -1)
johnedwardlee 3:8898bbbe00d6 75 {
johnedwardlee 3:8898bbbe00d6 76 speed = -1;
johnedwardlee 3:8898bbbe00d6 77 }else if (speed > 1)
johnedwardlee 3:8898bbbe00d6 78 {
johnedwardlee 3:8898bbbe00d6 79 speed = 1;
johnedwardlee 3:8898bbbe00d6 80 }
johnedwardlee 3:8898bbbe00d6 81
johnedwardlee 3:8898bbbe00d6 82 // Set the speed of the motors
johnedwardlee 3:8898bbbe00d6 83 left.speed(speed);
johnedwardlee 3:8898bbbe00d6 84 right.speed(speed);
johnedwardlee 3:8898bbbe00d6 85 }
johnedwardlee 3:8898bbbe00d6 86
jmar7 0:e8167f37725c 87 int main() {
johnedwardlee 3:8898bbbe00d6 88
johnedwardlee 3:8898bbbe00d6 89 imu.begin();
johnedwardlee 3:8898bbbe00d6 90 if (!imu.begin())
johnedwardlee 3:8898bbbe00d6 91 {
jmar7 0:e8167f37725c 92 pc.printf("Failed to communicate with LSM9DS1.\n");
johnedwardlee 3:8898bbbe00d6 93 exit(0);
jmar7 0:e8167f37725c 94 }
johnedwardlee 3:8898bbbe00d6 95
johnedwardlee 3:8898bbbe00d6 96 // Calibrate the IMU and set the biases
johnedwardlee 3:8898bbbe00d6 97 calibrate();
johnedwardlee 3:8898bbbe00d6 98
johnedwardlee 3:8898bbbe00d6 99 // Attach ticker to run every 10ms
johnedwardlee 3:8898bbbe00d6 100 // This handles the pid calculations for controlling the motors
johnedwardlee 3:8898bbbe00d6 101 PID.attach(&pid, 0.001);
johnedwardlee 3:8898bbbe00d6 102
johnedwardlee 3:8898bbbe00d6 103 // Program loop
johnedwardlee 3:8898bbbe00d6 104 while(1)
johnedwardlee 3:8898bbbe00d6 105 {
jmar7 0:e8167f37725c 106 }
jmar7 0:e8167f37725c 107 }