Project Bird Fish - Free Flight Robotics

//level 6 heading based turn

#include <PWMServo.h> //level 1

#include <i2c_t3.h> //level 2
#include <Adafruit_10DOF.h> //level2
#include <Adafruit_Sensor.h> //level2
#include <Adafruit_LSM303_U.h> //level2
#include <Adafruit_BMP085_U.h> //level2
#include <Adafruit_Simple_AHRS.h> //level2


#include <TinyGPS.h> //level4

PWMServo myservo1;  // level 1 create servo object servo1
PWMServo myservo2;  // level 1 create servo object servo2

TinyGPS gps; //level 4

// Create sensor instances. //level2
Adafruit_LSM303_Accel_Unified accel(30301); 
Adafruit_LSM303_Mag_Unified   mag(30302);
Adafruit_BMP085_Unified       bmp(18001);

// Create simple AHRS algorithm using the above sensors. //level2
Adafruit_Simple_AHRS          ahrs(&accel, &mag);

//  LEVEL 2 Update this with the correct SLP for accurate altitude measurements
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;

HardwareSerial Uart = HardwareSerial(); //level4
//--------------------
int pos1 = 140;
int pos2 = 50;
float poscom1 = pos1;
float poscom2 = pos2;
int n = 0;
int xint=0;
int loopcnt=100;
int turnlevel=50;

float heador = 0;
float headorOLD = 0;

float latgoal=37.362081;
float longgoal=-120.993190;

float brng = 0;
float brng1 = 0;
float turngain = .25;
float dist_calc = 0;
float ERRORhead = 1; // difference between heading and bearing
float ERabs = 1; 
float ERsplit=1;

//Use for Testing Altitude based logic! yea it mostly works
float testalt= 600; //meters

unsigned long course = 0;
//-------------------
void gpsdump(TinyGPS &gps);
void printFloat(double f, int digits = 2);

void setup() {
  Serial.begin(9600);
   Uart.begin(9600);
    
    myservo1.attach(3); //set servo 1 to pin 3 pwm level 1
    myservo2.attach(4); //set servo 2 to pin 4 pwm level 1
    
     
 delay(100);
  // Initialize the sensors. level2
  accel.begin();
  delay(10);
  mag.begin();
  delay(10);
  bmp.begin();
  delay(10);
  Serial.println("started");
  Serial.print("Servo1 Position, ");Serial.println(pos1);//level1
  Serial.print("Servo2 Position, ");Serial.println(pos2);//level1
     
  Serial.println("10 DOF Board AHRS Starting"); Serial.println("");//level2
}

void loop() {
  sensors_vec_t   orientation;
  delay(10);

  sensors_event_t event; //level3
  bmp.getEvent(&event); //level3
  
  myservo1.write(pos1);
  myservo2.write(pos2);

   bool newdata = false; //level4
  unsigned long start = millis(); //level4


   // Every 5 seconds we print an update //level4
  while (millis() - start < 5000) {
    if (Uart.available()) {
      char c = Uart.read();
       //Serial.print(c);  // uncomment to see raw GPS data
      if (gps.encode(c)) {
        newdata = true;
        //break;  // uncomment to print new data immediately!
      }
    }
  }

//turn circles when near waypoint
while(gps.f_altitude() < 500 && dist_calc < 1){

//while(testalt<15 && dist_calc < 0.8){
  delay(50);
myservo1.write(100);
myservo2.write(pos2);
Serial.print("right turn"); Serial.println();
Serial.print("Goal");Serial.println();
Serial.print(testalt);Serial.println();
delay(45000);
Serial.print("Hands Up");Serial.println();
myservo1.write(pos1);
myservo2.write(pos2);
delay(20000);
Serial.print("left turn");Serial.println();
myservo1.write(pos1);
myservo2.write(80);
delay(45000);
Serial.print("Hands Up");Serial.println();
myservo1.write(pos1);
myservo2.write(pos2);
delay(20000);
Serial.print("Distance to Goal: ");
    Serial.println(dist_calc);Serial.println();
}

//head for waypoint between release and waypoint

while((gps.f_altitude()) > 500 && gps.f_altitude() < 16150 ){
//while(testalt >15 && testalt < 1000){
  myservo1.write(pos1);
  myservo2.write(pos2);
  Serial.println();

brng = brng1;
 
//hands up for +/- 10 degrees on heading
  if(ERsplit < 180 && ERsplit > 160){
    turngain=1; 
   poscom1=pos1;
   poscom2 = pos2; 
   Serial.print("hands up");Serial.println();
}
 if (ERsplit < 20 && ERsplit > 0){
   turngain=1; 
   poscom1=pos1;
   poscom2 = pos2;
    Serial.print("hands up");Serial.println();
   }

   //

//small turn from +/- 10 to +/-45
  if(ERsplit < 160 && ERsplit > 135)
    turngain=0.4;
  if (ERsplit <45 && ERsplit >20)
    turngain=0.4;
    
//medium turn from +/-45 to +/-67.5
  if(ERsplit < 135 && ERsplit > 112.5)
    turngain=.7;
  if(ERsplit < 67.5 && ERsplit > 45)
    turngain=.7;

//fullturn from +/- 90 to +/- 67.5
  if(ERsplit < 112.5 && ERsplit > 67.5)
  turngain = .2;
 
Serial.print(" pos2   ");Serial.println(poscom2);
Serial.print(" pos1   ");Serial.println(poscom1);

if(ERRORhead > 0 && ERabs > 180){
  myservo2.write(poscom2);
  delay(500);
  Serial.println("Left (+)(>180)"); Serial.println();
}
if(ERRORhead > 0 && ERabs < 180){
  myservo1.write(poscom1);
  delay(500);
  Serial.println("Right (+)(<180)"); Serial.println();
}
if(ERRORhead < 0 && ERabs > 180){
  myservo1.write(poscom1);
  delay(500);
  Serial.println("Right (-)(>180)"); Serial.println();
}
if(ERRORhead < 0 && ERabs < 180){
  myservo2.write(poscom2);
  delay(500);
  Serial.println("Left (-)(<180)"); Serial.println();
}
//headorOLD = orientation.heading;
if (ahrs.getOrientation(&orientation))
  Serial.println("Orientation:  ");
//Serial.println(orientation.heading);
heador = course;//orientation.heading;// //200;// comment out orientation and put value to test
//heador = ((heador+headorOLD)/2);

//-----------------print heading .. in this case course. 
Serial.println(heador);

ERRORhead = brng - (heador);
ERabs = sqrt(sq(ERRORhead));
ERsplit = ERabs/2;

poscom1= floor(pos1-(turnlevel*turngain));
poscom2= floor(pos2+(turnlevel*turngain));

Serial.println("Error in Heading:   ");
Serial.print(ERRORhead); Serial.println();

//Serial.println("Error in split:   ");
//Serial.print(ERsplit); Serial.println();

//Serial.println("Turning Gain:  ");
//Serial.print(turngain); Serial.println();

xint=xint+1;
//Serial.println("Cnt: ");
//Serial.println(xint);
//Serial.println();

//level 3 pressure and alt
   // Serial.print("Pressure:    ");
   // Serial.print(event.pressure);
    //Serial.println(" hPa");
      /* Then convert the atmospheric pressure, and SLP to altitude         */
    /* Update this next line with the current SLP for better results      */
    float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
    Serial.print("Altitude:    "); 
    Serial.print(bmp.pressureToAltitude(seaLevelPressure,
                                        event.pressure)); 
                                        Serial.println();
                                       Serial.print(testalt);
    Serial.println(" m");
    Serial.println("");
    if (newdata) {
    Serial.println("Acquired Data");
    Serial.println("-------------");
    gpsdump(gps);
    Serial.println("-------------");
    Serial.println();
    delay(2000);
    
  }
}

//Full Stall 
while(gps.f_altitude()> 16150 && gps.f_altitude()<24400){
//while(testalt>1000 && testalt <10000) {
  //Stall
myservo1.write(70);
myservo2.write(120);
Serial.println("FullStall");
Serial.println();
  
   
   if (newdata) {
    Serial.println("Acquired Data");
    Serial.println("-------------");
    gpsdump(gps);
    Serial.println("-------------");
    Serial.println();

    delay(2000);
}

delay(1000);
}  
}
//GPS function Level 4 Calculate GPS alt, Current Position, Bearing to Goal

void gpsdump(TinyGPS &gps)
{
  
  long lat, lon;
  float flat, flon;
  unsigned long age, date, time, course;//, chars;
  int year;
  byte month, day, hour, minute, second, hundredths;
 
   
   float dist_calc=0;
  float dist_calc2=0;   
   
   gps.get_position(&lat, &lon, &age);
   gps.f_get_position(&flat, &flon, &age);
course = gps.course();
   float teta1 = radians(flat);
    float teta2 = radians(latgoal);
    float delta1 = radians(latgoal- flat);
    float delta2 = radians(longgoal- flon);
  
  
  Serial.print("Lat/Long(float): "); printFloat(flat, 5); Serial.print(", "); printFloat(flon, 5);
  Serial.print(" Fix age: "); Serial.print(age); Serial.println("ms.");
  Serial.print("Alt(meters): "); printFloat(gps.f_altitude()); Serial.println();
  Serial.print("Speed(knots): "); printFloat(gps.f_speed_knots()); Serial.println();

  gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
  Serial.print("Date: "); Serial.print(static_cast<int>(month)); Serial.print("/"); 
    Serial.print(static_cast<int>(day)); Serial.print("/"); Serial.print(year);
  Serial.print("  Time: "); Serial.print(static_cast<int>(hour)); Serial.print(":"); 
    Serial.print(static_cast<int>(minute)); Serial.print(":"); Serial.print(static_cast<int>(second));
    Serial.print("."); Serial.print(static_cast<int>(hundredths));
     Serial.println(" ");

//calcuate bearing
  float y = sin(delta2) * cos(teta2);
    float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
    brng1 = atan2(y,x);
    brng1 = degrees(brng1);// radians to degrees
    brng1 = ( ((int)brng1 + 360) % 360 ); 
     
     Serial.print("Bearing to Goal: ");
    Serial.println(brng1);

      //calculate range
dist_calc = (sin(delta1/2.0)*sin(delta1/2.0));
dist_calc2= cos(teta1);
dist_calc2*=cos(teta2);
dist_calc2*=sin(delta2/2.0);
dist_calc2*=sin(delta2/2.0);
dist_calc +=dist_calc2;

dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));

dist_calc*=6371000.0 / 1000; //Converting to meters -> km
//Serial.println(dist_calc);

Serial.print("Distance to Goal: ");
    Serial.println(dist_calc);


}

  void printFloat(double number, int digits)
{
  // Handle negative numbers
  if (number < 0.0) {
     Serial.print('-');
     number = -number;
  }

  // Round correctly so that print(1.999, 2) prints as "2.00"
  double rounding = 0.5;
  for (uint8_t i=0; i<digits; ++i)
    rounding /= 10.0;
  
  number += rounding;

  // Extract the integer part of the number and print it
  unsigned long int_part = (unsigned long)number;
  double remainder = number - (double)int_part;
  Serial.print(int_part);

  // Print the decimal point, but only if there are digits beyond
  if (digits > 0)
    Serial.print("."); 

  // Extract digits from the remainder one at a time
  while (digits-- > 0) {
    remainder *= 10.0;
    int toPrint = int(remainder);
    Serial.print(toPrint);
    remainder -= toPrint;
  }
}

There are a couple things that I learned. There are also a few things that make paragliders an advantage when trying to automate. They are pendulum stable. This is a unique advantage.

There is a funny joke about how not many things when they break become almost as useful. The punch line.. is when the escalator breaks.. you have. stairs.. thank you. ill take the stairs. 

Why do I make this joke. When thinking about distributed sensors, those who take to the ocean, such as Saildrones or Sofar are at a bit of an advantage. If they loose the ability to steer or be mobile, they still can be a valuable data source floating along.. 

I make this comparison because a paraglider, unlike an airplane, quadcopter, or helicopter, is sometimes best left alone... hahah. 

When instructing paragliding I mention there are three things to manage. The equipment, the weather, and mindset. All of these are focused on the pilot managing their future self. 

If the air is calm. The wing is open.  Everything will work great. With no input. This is an interesting insight. 

It leads into the code, I promise. It means. do less. Do as little as you can to maintain heading. 

What about collapses, managing wing pressure you ask? Id say. For now. We leave all that out. There is more than enough to take care of before managing all that. I have a plan... Ill write about it.. but not yet. 

So that leaves us with.. 

Where do you want to go.. Where does the robot think it is, where does it think its going, How do I get from Robot State (where I am) to Robot State (where I want to go) 

To our advantage. We only have two outputs. Left and Right. 

We only have one set of inputs. GPS based bearing, GPS location. 

To make this all as simple as possible I pre-prescribed a final location waypoint. Why? so that I didnt have to use a compass. Each loop I took the error between bearing to the waypoint GPS location and the current GPS bearing. That would become the angle required to steer back to course.. 

With the error angle we have some choice to make!!! 

Where would you imagine the largest input would be... 

Lets walk through it.. 

Imagine a large angle like 180 degrees. Would you slam on a control to get back?  

What about a very small angle like 5 degrees. Should you even do anything? 

I drew a compass for all angles. 

I stared at it for a long time. Imagining what I do flying... I  have the ability to correct.. but my robot does not.. What would I tell a student on the radio.. 

I will post what I choose on this log when I dig up the picture.. or will just redraw it. Less is more.. Depending on altitude.. who cares how long it takes.. It only matters if you are not making it where you want to go.. Many times flying people put in big ears.. or spiral to get down.. when they could calmly go find sink or fly somewhere else.. This is important to layer onto how the robot is programed. 

What about modes?? Drop Mode. Recover Mode. Waypoint Mode. Thermal Mode. Team... etc etc. 

Ok well.. to make it easy again.. with limited sensing. Lets use modes triggered by altitude. 

We designed the balloon to go to 80,000ft.. with a planned drop at 60,000ft. 

At max altitude the air density is  = 0.0003097 kg/m^3

At Sealevel the air density is = 1.225 kg/m^3

If we can someday show this all works above 114,000ft then well. We also have a Martian explorer. I digress.

Ok.. Here is the challenge. Our speeds will be very different. Our control inputs will also have to vary.. by how much??!?!?! we actually dont know.. Less is more.. 

From later testing I learned about the torque issue with the servos.. Only from later testing.. I think if I had the right servos on the high altitude flight I would have fallen out of the sky.. BUT the servos were not capable of over controlling to the point of spin or stall.. WHAT I got soooo freaking lucky.. Happy accident for sure. 

I've uploaded what I used. as well as the new code on github. 

The first design was born out of urgency. After some lessons learned. I CAD'd up the foam design so that it could be 3d-printed and shared. 

Limiting support material at first was a goal, but then.. well.. I totally abandoned it. I needed interfaces where I needed them, and strength where I needed it.. Oh how I wish I had the time and patience for elegance.. Well here you go. 

One thing I wanted to add was a speed system. This is an accelerator, that changes the angle of incidence of the wing relative to the robot pilot. The effect is that the system flies faster. The glide is reduced, but the Freedom of Movement is increased in situations with headwind. I made small pullies with even smaller bearings, why? because i tried to find tiny bearings! it was awesome. 

One thing that was discovered primarily due to scaling up the wings, was that when loading was increased the original 9g servos just didnt have enough torque to steer. This also was noted when flying at Pacifica, California in 20mph of wind. 

More POWER! I added larger servos, but in doing so a number of issues popped up. 

1. The noise was so loud because of the constraining of the airflow on the EDF

2. The control arms needed a redesign. 

3. More power use. 

4. there was an unexpected unplanned benefit from undersizing the servos.. In high speed high load situations.. the wing.. well wouldnt spin or stall due to input.. When flying lightly loaded this was a characteristic that was noted often. The limited torque would mean almost no displacement for the servo.. In 30mph of wind.. thats perfectly fine.. 

5. My issue with 4 is that its not controlled.  


------
These units are to understand the wings. Not for the free flight side of things, but its not everyday that you can deploy from a balloon, or you must have a UAV or other method, building, bridge etc. I did not have these. 

Speed system on the single surface, helped but also led to many frontal collapses.. In the end.. Im not sure it is the right fit. Also it was critical to not induce any torque from the speed system, or the pilot would rotate and the thrust vector would change. This could be used as an advantage, but I choose to reduce it to near zero. 

The idea was to build these out to be super reliable. share with the world. Get students and universities developing the software further. Work on modes and automation, as well as fleet management, deployment and collection. 

How do you create a process everyone has access to. Well. To tune the brakes. I figured I need load in some direction at minimum. So I hung the whole thing from my ceiling. This gave me a great starting point that I later adjusted. 

I was able to get four models in the hands of pilots for good, and at least three more test pilots trained up. It was really fun. Once everything worked. One VIP aspect was a switch to not have to pull the XT60 each time.. since. that. for me.. um breaks things.. models that are delicate to be specific. Well the high amps of the EDF fried the switch and wings were almost lost in terrible places... 

This is slightly out of order, but will allow the reader to understand some of the context for the current work.

For the automated paraglider. The single most challenging part was finishing before our launch date... then after that, i'd say the falling with no air.... This was expected, but to what extent was not known at the time. The goal was to prepare the deployment bag in a way that there was a high probability of success. The design of the robot had a wide riser separation, approaching the limit of what was a good idea.. This allowed for the robot to naturally untwist, if twists occured.... They DID.. here is a moment of silence before some brief chaos. 

The early work started with making my own wing. That was hard. I then went with getting an off the shelf wing. I will return to wing design, but I realized it was more critical to nail the full chain of events.