Nick Restivo / Mbed 2 deprecated ParamotorControl

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:   PwmIn mbed Servo

main.cpp@0:9e0de96e4dfd, 2018-12-13 (annotated)

Committer:
bignick26
Date:
Thu Dec 13 21:30:04 2018 +0000
Revision:
0:9e0de96e4dfd
The beginning of a flight controller for a paramotor drone

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bignick26 0:9e0de96e4dfd 1 //Nicholas Restivo
bignick26 0:9e0de96e4dfd 2 #include "mbed.h"
bignick26 0:9e0de96e4dfd 3 #include "PwmIn.h"
bignick26 0:9e0de96e4dfd 4 #include "Servo.h"
bignick26 0:9e0de96e4dfd 5 #include "LSM9DS1.h"
bignick26 0:9e0de96e4dfd 6
bignick26 0:9e0de96e4dfd 7 #define PI 3.14159
bignick26 0:9e0de96e4dfd 8
bignick26 0:9e0de96e4dfd 9 Servo Rout(p22);
bignick26 0:9e0de96e4dfd 10 Servo Lout(p21);
bignick26 0:9e0de96e4dfd 11
bignick26 0:9e0de96e4dfd 12 Serial pc(USBTX, USBRX);
bignick26 0:9e0de96e4dfd 13
bignick26 0:9e0de96e4dfd 14 PwmIn elev(p16);
bignick26 0:9e0de96e4dfd 15 PwmIn right(p17);
bignick26 0:9e0de96e4dfd 16 PwmIn left(p18);
bignick26 0:9e0de96e4dfd 17 float Rin = 0;
bignick26 0:9e0de96e4dfd 18 float Lin = 0;
bignick26 0:9e0de96e4dfd 19 float Ein = 0;
bignick26 0:9e0de96e4dfd 20 bool RCon = true;
bignick26 0:9e0de96e4dfd 21
bignick26 0:9e0de96e4dfd 22 float pitch;
bignick26 0:9e0de96e4dfd 23
bignick26 0:9e0de96e4dfd 24 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
bignick26 0:9e0de96e4dfd 25
bignick26 0:9e0de96e4dfd 26
bignick26 0:9e0de96e4dfd 27 float getAttitude(float ax, float ay, float az)
bignick26 0:9e0de96e4dfd 28 {
bignick26 0:9e0de96e4dfd 29 float roll = atan2(ay, az);
bignick26 0:9e0de96e4dfd 30 float pitch_res = atan2(-ax, sqrt(ay * ay + az * az));
bignick26 0:9e0de96e4dfd 31
bignick26 0:9e0de96e4dfd 32 // Convert everything from radians to degrees:
bignick26 0:9e0de96e4dfd 33 pitch_res *= 180.0 / PI;
bignick26 0:9e0de96e4dfd 34 roll *= 180.0 / PI;
bignick26 0:9e0de96e4dfd 35 if (pitch_res < -3.0 || pitch_res > 3.0) pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch_res,roll);
bignick26 0:9e0de96e4dfd 36 return pitch_res;
bignick26 0:9e0de96e4dfd 37 }
bignick26 0:9e0de96e4dfd 38
bignick26 0:9e0de96e4dfd 39 int main() {
bignick26 0:9e0de96e4dfd 40
bignick26 0:9e0de96e4dfd 41 Rout.calibrate(0.0009, 90.0);
bignick26 0:9e0de96e4dfd 42 Lout.calibrate(0.0009, 90.0);
bignick26 0:9e0de96e4dfd 43
bignick26 0:9e0de96e4dfd 44 //Accelerometer
bignick26 0:9e0de96e4dfd 45 IMU.begin();
bignick26 0:9e0de96e4dfd 46 if (!IMU.begin()) {
bignick26 0:9e0de96e4dfd 47 pc.printf("Failed to communicate with LSM9DS1.\n");
bignick26 0:9e0de96e4dfd 48 }
bignick26 0:9e0de96e4dfd 49 IMU.calibrate(1);
bignick26 0:9e0de96e4dfd 50 while(!IMU.accelAvailable());
bignick26 0:9e0de96e4dfd 51 IMU.readAccel();
bignick26 0:9e0de96e4dfd 52 pitch = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
bignick26 0:9e0de96e4dfd 53
bignick26 0:9e0de96e4dfd 54 while(1) {
bignick26 0:9e0de96e4dfd 55
bignick26 0:9e0de96e4dfd 56 if (RCon) {
bignick26 0:9e0de96e4dfd 57
bignick26 0:9e0de96e4dfd 58 printf("Duty Cycle: %f \n", Ein);
bignick26 0:9e0de96e4dfd 59
bignick26 0:9e0de96e4dfd 60 // Input Values & Round
bignick26 0:9e0de96e4dfd 61 Lin = float(int(left.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .75 - 1
bignick26 0:9e0de96e4dfd 62 Rin = float(int(right.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .75 - .96
bignick26 0:9e0de96e4dfd 63 Ein = float(int(elev.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .74 - .53
bignick26 0:9e0de96e4dfd 64
bignick26 0:9e0de96e4dfd 65 //Scale to controller
bignick26 0:9e0de96e4dfd 66 Lin = 1.0 - (Lin - 0.75) * 4.0; // 1 when stick is centered
bignick26 0:9e0de96e4dfd 67 Rin = (Rin - 0.75) * 4.7619; // 0 when stick is centered
bignick26 0:9e0de96e4dfd 68 Rin = float(int(Rin * 100 + 0.5)) / 100;
bignick26 0:9e0de96e4dfd 69 // if (Ein > 0.65) {
bignick26 0:9e0de96e4dfd 70 // Ein = 0.0;
bignick26 0:9e0de96e4dfd 71 // }
bignick26 0:9e0de96e4dfd 72 if (Ein > 0 && Ein <= 0.65) {
bignick26 0:9e0de96e4dfd 73 Ein = (1.0 - (Ein - .53) * 8.333);
bignick26 0:9e0de96e4dfd 74 Ein = float(int(Ein * 100 + 0.5)) / 100; // 0 when centered (and a little below)
bignick26 0:9e0de96e4dfd 75 Lin = 1.0 - Ein;
bignick26 0:9e0de96e4dfd 76 Rin = Ein;
bignick26 0:9e0de96e4dfd 77 }
bignick26 0:9e0de96e4dfd 78
bignick26 0:9e0de96e4dfd 79 Lout.write(Lin); //float percent .75 - 1... -> 0 - .25 -> 0 - 1.0
bignick26 0:9e0de96e4dfd 80 Rout.write(Rin); //float percent .75 - .96... -> 0 - .21 -> 0 - 1.0
bignick26 0:9e0de96e4dfd 81
bignick26 0:9e0de96e4dfd 82 }
bignick26 0:9e0de96e4dfd 83
bignick26 0:9e0de96e4dfd 84 else {
bignick26 0:9e0de96e4dfd 85 //To-Do!
bignick26 0:9e0de96e4dfd 86 //integrate accelerometer readings to allow autonomous control
bignick26 0:9e0de96e4dfd 87 //while(!IMU.accelAvailable());
bignick26 0:9e0de96e4dfd 88 IMU.readAccel();
bignick26 0:9e0de96e4dfd 89 pitch = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
bignick26 0:9e0de96e4dfd 90 if (pitch > 15.0) {
bignick26 0:9e0de96e4dfd 91 Rout.write(1.0);
bignick26 0:9e0de96e4dfd 92 }
bignick26 0:9e0de96e4dfd 93 else if (pitch < -15.0) {
bignick26 0:9e0de96e4dfd 94 Lout.write(0.0);
bignick26 0:9e0de96e4dfd 95 }
bignick26 0:9e0de96e4dfd 96 else {
bignick26 0:9e0de96e4dfd 97 Rout.write(0.0);
bignick26 0:9e0de96e4dfd 98 Lout.write(1.0);
bignick26 0:9e0de96e4dfd 99 }
bignick26 0:9e0de96e4dfd 100
bignick26 0:9e0de96e4dfd 101 }
bignick26 0:9e0de96e4dfd 102
bignick26 0:9e0de96e4dfd 103 Lin = float(int(left.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .75 - 1
bignick26 0:9e0de96e4dfd 104 if (Lin == 0.0) {
bignick26 0:9e0de96e4dfd 105 RCon = false;
bignick26 0:9e0de96e4dfd 106 }
bignick26 0:9e0de96e4dfd 107 else {
bignick26 0:9e0de96e4dfd 108 RCon = true;
bignick26 0:9e0de96e4dfd 109 }
bignick26 0:9e0de96e4dfd 110 }
bignick26 0:9e0de96e4dfd 111 }