sharknado.ino

#include <Adafruit_GPS.h>
#include "location.h"
#include <string>
#include <Sabertooth.h>
#include <LSM303.h>
#include <Wire.h>
#include <PID_v1.h>
#include <DueTimer.h>
#include <Servo.h> //must include here to to link in payloaddumper.cpp
#include "ProximitySensor.h"
#include "PayloadDumper.h"
#include "BeaconSensor.h"
#define LAB_CAL //calibrate the compass for home operation
#define ARDUINO
using namespace sharknado;


enum STATES { START, SEARCH, BEACON, ESCAPE, DONE_WAIT };
int state; //0,1,2 = search, beacon, obstacle
int next_state;

#define MAX_SPEED 20//40
#define SLOW_SPEED 10//20
#define GPS_SERIAL Serial2 //had to hack adafruit lib to use Serial2
#define GPS_LED 53
#define MAG_LED 51
#define BTN_PIN 49

//ultrasonic vars
#define trigPinL 32     // Pin 12 trigger output
#define trigPinR 34
#define trigPinC 36
#define echoPinL 33                                    // Pin 2 Echo input
#define echoPinR 35
#define echoPinC 37
volatile long echo_startL = 0;                         // Records start of echo pulse 
volatile long echo_endL = 0;                           // Records end of echo pulse
volatile long echo_durationL = 0;                      // Duration - difference between end and start
volatile long echo_startR = 0;                         // Records start of echo pulse 
volatile long echo_endR = 0;                           // Records end of echo pulse
volatile long echo_durationR = 0;                      // Duration - difference between end and start
volatile long echo_startC = 0;                         // Records start of echo pulse 
volatile long echo_endC=  0;                           // Records end of echo pulse
volatile long echo_durationC = 0;                      // Duration - difference between end and start
int left_dist;
int center_dist;
int right_dist;


LSM303 compass;
Location loc;
ProximitySensor ultra1; //left
ProximitySensor ultra2; //center
ProximitySensor ultra3; //right
PayloadDumper payload;
BeaconSensor beacon_sensor;
Sabertooth ST(128);
Adafruit_GPS GPS(&GPS_SERIAL);

Location::latlng target1= {32.777474f, -117.069745f};
Location::latlng target2= {32.774143f, -117.071791f};
Location::latlng target3= {32.774140f, -117.070708f};
Location::latlng home=    {32.774387f, -117.070701f};

int target_index=0;
Location::latlng targets [] = {target1};

//distance and heading to target
float current_target_distance=0;
double current_target_heading=0;
double compass_reading = 0;

//trusted lat lng
Location::latlng current_latlng;
Location::latlng current_gps_latlng;

//this is the trusted heading that stearing will rely upon. It should be the output of some (Kalman) filter
//which will be a function of magnetometer and GPS
double current_heading=0;
double motor_turning_coeff=0; //current val of motor steering interface
float current_gps_heading=0;
float current_magnetometer_heading=0;
double expected_heading=0;

//trusted speed -- function of encoders and GPS
double current_speed=0;
double motor_speed_coeff=0; //current val of motor speed interface
float current_gps_speed=0;
float current_encoder_speed=0;
double expected_speed=0;
bool northflag = false;

//maybe update these values w/ interrupt?
bool collision=false; //call ultrasonic proximity routine
bool beacon_range=false; //call bacon proximity routine


//Specify the links and initial tuning parameters
//PID heading_PID(&current_heading, &motor_turning_coeff, &expected_heading, 2,.1,0, DIRECT);
PID  heading_PID(&compass_reading, &motor_turning_coeff, &current_target_heading, 2,.1,0, DIRECT);
PID speed_PID  (&current_speed, &motor_speed_coeff, &expected_speed, 2,5,1, DIRECT);



void setup() {
    //start in start mode
    next_state=START;

    // Set uop led Pins
    pinMode(GPS_LED,OUTPUT);
    pinMode(MAG_LED,OUTPUT);
    pinMode(BTN_PIN,INPUT);
    digitalWrite(GPS_LED, LOW);
    digitalWrite(MAG_LED, LOW);
    digitalWrite(BTN_PIN, HIGH); //writing to an input?

    // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
    Serial.begin(9600);
    Serial.println("Sharknado GPS naviation routine!");
    GPS.begin(9600);
    GPS_SERIAL.begin(9600);
    GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
    // Set the update rate
    GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);   // 1 Hz update rate
    // Request updates on antenna status, comment out to keep quiet
    GPS.sendCommand(PGCMD_ANTENNA);

    delay(1000);
    // Ask for firmware version
    GPS_SERIAL.println(PMTK_Q_RELEASE);

//initialize sabertooth
    Serial.begin(9600);
    SabertoothTXPinSerial.begin(9600); // This is the baud rate you chose with the DIP switches.
    ST.autobaud();
    ST.drive(0); // The Sabertooth won't act on mixed mode until
    ST.turn(0);


    Wire.begin();
    bool compass_status=compass.init();
    Serial.print("Compass status: ");
    Serial.println(compass_status);
    compass.enableDefault();

#ifdef HOME_CAL
    Serial.println("Calibrating compass for home mode");
    compass.m_min = (LSM303::vector<int16_t>) {
        -1028,   -784,  +2487
    };
    compass.m_max = (LSM303::vector<int16_t>) {
        +1121,  +1215,  +3276
    };
#else
//    //min: {  -617,  -1118,   +988}    max: { +1495,  +1025,  +1155} lab
//    //min: {  -752,  -1401,  +2073}    max: { +1224,   +743,  +2942}
//    //min: { -1970,  -2568,  +1301}    max: { +1620,   +912,  +1950} field
//    //min: { -1563,  -1675,  +1059}    max: { +1797,  +1509,  +1723} outside parking
//    //min: { -1028,   -784,  +2487}    max: { +1121,  +1215,  +3276} home
//    //min: { -2024,  -1198,   +522}    max: {  +415,  +1047,  +1369} lab friday
//    //min: { -2068,  -2272,   +360}    max: { +1358,   +783,  +1189} //outsite friday
//    min: { -1809,  -1856,   +673}    max: { +1819,  +1571,  +1359}  field wed


//
    Serial.println("Calibrating compass for lab mode");
    compass.m_min = (LSM303::vector<int16_t>) {
        -1253,  -2072,   -244
    } ;
    compass.m_max = (LSM303::vector<int16_t>) {
        +2042,   +774,   +674
    };
#endif


    //timer interrupt for GPS
    Timer.getAvailable().attachInterrupt(gps_interrupt).start(1000); // Calls every 50ms

    //initialize PIDs
    heading_PID.SetMode(AUTOMATIC);
    heading_PID.SetOutputLimits(-7,7);

    speed_PID.SetMode(AUTOMATIC);
    speed_PID.SetOutputLimits(0,50);
    
    //ultras
  pinMode(trigPinL, OUTPUT);                           // Trigger pin set to output
  pinMode(trigPinR, OUTPUT);
  pinMode(trigPinC, OUTPUT);
  pinMode(echoPinL, INPUT);    
  pinMode(echoPinR, INPUT); 
  pinMode(echoPinC, INPUT); // Echo pin set to input
  Timer6.attachInterrupt(trigger_pulse).start(250000);     //sends a pulse every 250 ms and lasts 50 us
 
  attachInterrupt(echoPinL, echo_interruptL, CHANGE);  // Attach interrupt to the sensor echo input
  attachInterrupt(echoPinR, echo_interruptR, CHANGE);  // Attach interrupt to the sensor echo input
  attachInterrupt(echoPinC, echo_interruptC, CHANGE);  // Attach interrupt to the sensor echo input

}


void loop() {
  
   

    
     Serial.print( "L= ");Serial.print(left_dist);               // Print the distance in centimeters
     Serial.print(" C= ");Serial.print(center_dist);               // Print the distance in centimeters
     Serial.print(" R= ");Serial.print(right_dist);               // Print the distance in centimeters
     Serial.print(", ");
     Serial.println(check_collision()); //sets the collison
 

    //next state logic
    state=next_state;

    if (state==START) 		start_routine();
    else if(state==SEARCH) 	search_routine();
    else if(state==BEACON) 	beacon_search_routine();
    else if(state==ESCAPE)	escape_routine();
    else if(state==DONE_WAIT)   done_wait();
    
    

}//end loop

bool mag_ready=false;
bool gps_ready=false;
bool shark_flag=false;
void start_routine()
{
    //initialize the motors to 0
    ST.drive(0);
    
    
    //wait for Mag. to initialize
    compass.read();
    compass_reading=compass.heading();
    current_heading = aconv(compass_reading);
    if (current_heading!=current_heading) //wait for magnetometer reading, nan != nan is always true
    {
        Serial.print("Magnetometer is not set up...Please unplug and try again");
        digitalWrite(MAG_LED, LOW);
    }
    else //mag is ready
    {
        mag_ready=true;
        digitalWrite(MAG_LED, HIGH); //turn Yellow LED on
    }

    //GPS Check
    if (!GPS.fix)
    {
      if (GPS.newNMEAreceived()) 
        if (!GPS.parse(GPS.lastNMEA()))
           
        Serial.println("No GPS Fix...Please Hold");
        digitalWrite(GPS_LED, LOW);
    }
    else //GPS fix initiated
    {
        gps_ready=true;
        digitalWrite(GPS_LED, HIGH); //Turn Red LED on
    }


    //Button check
    if(digitalRead(BTN_PIN)==HIGH && shark_flag==false && gps_ready && mag_ready) //btn is pressed
    {
        Serial.println("I'm waiting for you to push the button!");

    }
    else if(digitalRead(BTN_PIN)==LOW)
    {
        shark_flag=true;
	
    }
    
    if(shark_flag && mag_ready && gps_ready)
    {
      Serial.println("Starting sharknado...");
      digitalWrite(GPS_LED, LOW); //turn LEDs off
      digitalWrite(MAG_LED, LOW);

       next_state=SEARCH;
       delay(1000); //wait a second then GO!
    }

}

void search_routine()
{
  
    if(check_collision()) 
    {  
      next_state=ESCAPE; 
      return;
    }
    else if(beacon_range) next_state=BEACON;


    if (GPS.newNMEAreceived()) {
        if (!GPS.parse(GPS.lastNMEA()))
            return;
    }
    if (GPS.fix) {
        float lat = GPS.latitude;
        float lon = GPS.longitude;
        current_latlng.lat=loc.convertDegMinToDecDeg(lat);
        current_latlng.lng=-loc.convertDegMinToDecDeg(lon);
        current_gps_heading=GPS.angle;
        current_gps_speed=GPS.speed;
        update_dist_and_heading_to_target();
    }
    else
    {
        Serial.println("no fix on the gps, transitioning back to start state");
        next_state=START; 
        return;
        
    }

    //compute expected shark speed every cycle
    update_expected_speed();

    if(current_target_distance < 3) next_state=BEACON; //hack until we get beacon RF sensor

    Serial.print(current_latlng.lat, 9);
    Serial.print(", ");
    Serial.print(current_latlng.lng, 9);
    Serial.print(", ");
    Serial.print("distance: ");
    Serial.print(current_target_distance);
    Serial.print(", ");
    Serial.print("GPS heading: ");
    Serial.print(current_target_heading);

    //heading PID update
    compass.read();
    compass_reading=compass.heading();
    //compass error checking
    if (compass_reading!=compass_reading) //wait for magnetometer reading, nan != nan is always true
    {
        Serial.println("Magnetometer is broken, transitioning back to START STATE");
        next_state=START;
        return;
    }
    if (northflag) {
        compass_reading = aconv(compass_reading); //if in northern hemisphere must convert. must have this because compass.read is called after distance update function
    }
    heading_PID.Compute();
    double turning=-motor_turning_coeff; //negate to steer correctly
    ST.turn(turning);

    //speed PID update
//    speed_PID.Compute();
    ST.drive(expected_speed);

    int coeff=expected_speed/5;
    heading_PID.SetOutputLimits(-coeff,coeff); //turning is  a function of speed

    //print heading info
    Serial.print(", motor_turning_coeff:\t");
    Serial.print(turning);
    Serial.print(", ");
    Serial.print("current heading:\t");
    Serial.print(compass_reading);
    Serial.print(", expected speed: \t");
    Serial.println(expected_speed);



}

void beacon_search_routine()
{
    //Drive forward slowly until RSSI is > 90
    //then drop ball
    ST.drive(SLOW_SPEED);

    //drop golf ball at some point
    //this is hacked to return random number between 50 and 100
    int rssi = beacon_sensor.sample();
    Serial.print("RSSI: ");
    Serial.println(rssi);

    if(rssi>90)
    {
        ST.drive(0); //stop
        payload.dump(); //payload lib

        //set the next target 	//set next state
        target_index++;
        if( (target_index % 1) == 0) next_state=DONE_WAIT; //you are home, stop
        else next_state = SEARCH; //search for next target
    }
}

void escape_routine()
{
    if(check_collision()) next_state=ESCAPE;
    else next_state=SEARCH; //clear of obstacle, go back to normal search


    //implement escape logic?
    //just slow down and make a sharp left or right turn
    ST.drive(MAX_SPEED);
    
    int turn_coeff=MAX_SPEED/5;
    
    if(left_dist<50)
    {
       ST.turn(-turn_coeff);
    }
    else if(right_dist <50)
    {
      ST.turn(turn_coeff);
    }
    else if(center_dist < 100)
    {
      ST.turn(-turn_coeff);
    }
    //ST.turn(MAX_TURN);
}

//this is just the busy wait
//the robot shouldn't do anything
void done_wait()
{
    ST.drive(0);
    Serial.println("finished work, home");
    return;
}

void update_dist_and_heading_to_target()
{
    float results [3];
    loc.computeDistanceAndBearing(current_latlng.lat, current_latlng.lng, targets[target_index].lat, targets[target_index].lng, results);
    current_target_distance = results[0];
    current_target_heading = results[1];
    Serial.println(current_target_heading);
    
    //Checks if heading is in the southern hemisphere
    if (90 < current_target_heading && current_target_heading < 270)  northflag=false; // if it is then north flag is false
    else {
        compass_reading = aconv(compass_reading);//convert to northern heading or -180 to 180
        current_target_heading = aconv(current_target_heading);
        northflag=true;
    }
}

void update_expected_speed()
{
    if(current_target_distance > 9)       expected_speed=MAX_SPEED; //full speed for up to 20 meters to beacon
    else if(current_target_distance > 3)  expected_speed=SLOW_SPEED; //go slow for 17 meters
    else if(current_target_distance > 0)  expected_speed=0; //this percission is questionable
    else if(current_target_distance <=0)  expected_speed=0; //stop, we passed beacon.
}


void gps_interrupt()
{
    GPS.read();
}


//this scales a 0-360 heading to -180-180 for the PID library input
float aconv(float theta)
{
    return fmod((theta + 180.0f), 360.0f) - 180.0f;
}


void trigger_pulse()
{
     //sets all pins to high or triggers the ultrasonic
     digitalWrite(trigPinL, HIGH);
     digitalWrite(trigPinR, HIGH);
     digitalWrite(trigPinC, HIGH);
     Timer7.attachInterrupt(goLow).start(50);     //sets a  timer that will make the pulse go low after 50 us
     
}

void goLow()
{
  //sets all the pins to low
  //Serial.println("pin goes low");
  digitalWrite(trigPinL, LOW);
  digitalWrite(trigPinR, LOW);
  digitalWrite(trigPinC, LOW);
  Timer7.detachInterrupt();                        //detachs the interupt so that it won't go off again until initialized
  
}

void echo_interruptL()                          
{
  switch (digitalRead(echoPinL))                     // Test to see if the signal is high or low
  {
    case HIGH:                                      // High so must be the start of the echo pulse
      echo_endL = 0;                                 // Clear the end time
      echo_startL = micros();                        // Save the start time
      break;
      
    case LOW:                                       // Low so must be the end of the echo pulse
      echo_endL = micros();                          // Save the end time
      echo_durationL = echo_endL - echo_startL;        // Calculate the pulse duration
      left_dist = echo_durationL / 58;
      break;
  }
}

void echo_interruptR()
{
  switch (digitalRead(echoPinR))                     // Test to see if the signal is high or low
  {
    case HIGH:                                      // High so must be the start of the echo pulse
      echo_endR = 0;                                 // Clear the end time
      echo_startR = micros();                        // Save the start time
      break;
      
    case LOW:                                       // Low so must be the end of hte echo pulse
      echo_endR = micros();                          // Save the end time
      echo_durationR = echo_endR - echo_startR;        // Calculate the pulse duration
      right_dist=echo_durationR / 58;
      break;
  }
}

void echo_interruptC()
{
  switch (digitalRead(echoPinC))                     // Test to see if the signal is high or low
  {
    case HIGH:                                      // High so must be the start of the echo pulse
      echo_endC = 0;                                 // Clear the end time
      echo_startC = micros();                        // Save the start time
      break;
      
    case LOW:                                       // Low so must be the end of hte echo pulse
      echo_endC = micros();                          // Save the end time
      echo_durationC = echo_endC - echo_startC;        // Calculate the pulse duration
      center_dist = echo_durationC / 58;
      break;
  }
}

bool check_collision()
{
  bool left=false;
  bool center=false;
  bool right=false;
  
  int threshold=150;
  
  if(left_dist < threshold/3) left=true;
  if(right_dist < threshold) right=true;
  if(center_dist < threshold/3) center=true; 
  
  if(left || center || right) return true;
  return false;
  
  
}