RCbot

// RCbot
// this code is provided "as is"

/****************************************
 * Setup                                *
 ****************************************/
// AVR Runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>

// APM Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h>             // ArduPilotMega RC library
#include <AP_GPS.h>             // ArduPilot GPS library
#include <Wire.h>               // Arduino I2C library
#include <DataFlash.h>          // ArduPilotMega flash memory library
#include <AP_ADC.h>             // ArduPilotMega analog to digital converter library
#include <APM_BMP085.h>         // ArduPilotMega BMP085 (barometer) library
#include <AP_Compass.h>         // ArduPilotMega magnetometer library
#include <AP_Math.h>            // ArduPilotMega vector/matrix math library
#include <AP_IMU.h>             // ArduPilotMega IMU library
#include <AP_DCM.h>             // ArduPilotMega DCM library
#include <PID.h>                // PID library
#include <AP_RangeFinder.h>     // Rangefinder library

// Additional Data Types
struct wp_xy {
    int        id;              // waypoint ID
    int32_t     x;              // x-coordinate relative to reference (cm)
    int32_t     y;              // y-coordinate relative to reference (cm)
};

// Math Constants
#define deg2rad(deg) (deg*0.01745329252)  // *pi/180
#define rad2deg(rad) (rad*57.2957795131)  // *180/pi

// Configuration
#define RF_PIN            1007      // rangefinder analog input pin (pitot tube input)
#define ENC_A_PIN           15      // aka PJ0/PCINT9 (telemetry port)
#define ENC_B_PIN           14      // aka PJ1/PCINT10 (telemetry port)

#define A_LED_PIN           37
#define B_LED_PIN           36
#define C_LED_PIN           35

#define FTDI_BAUDRATE   115200      // FTDI serial interface baudrate
#define GPS_BAUDRATE     38400      // GPS serial interface baudrate

#define ID_PAN               0      // Channel 0 is rangefinder pan (output)
#define ID_SAFETY            1      // Channel 2 is safety (input)
#define ID_STEER             1      // Channel 1 is steering
#define ID_THROTTLE          2      // Channel 2 is throttle

/****************************************
 * Parameters and Variables             *
 ****************************************/
// coordinate parameters (PASADENA)
//#define ref_lat      341300000          // reference latitude * 10^7
//#define ref_lng    -1181400000          // reference longitude * 10^7
//#define dlng2x(dlng) (dlng*0.92234300)   // delta longitude (deg * 10^7) to x coordinate (cm)
//#define dlat2y(dlat) (dlat*1.10924946)  // delta latitude (deg * 10^7) to y coordinate (cm)
//#define declination 12.617              // in degrees
// coordinate parameters (BOULDER)
#define ref_lat      400648660          // reference latitude * 10^7
#define ref_lng    -1052100770          // reference longitude * 10^7
#define dlng2x(dlng) (dlng*0.85313000)   // delta longitude (deg) to x coordinate (cm)
#define dlat2y(dlat) (dlat*1.11035939)  // delta latitude (deg) to y coordinate (cm)
#define declination 9.083               // in degrees

#define COMPASS_MOUNT_OFFSET 3.5          // degrees to add to compensate for mounting offset

// servo details
#define PAN_TRIM          1500
#define STEER_TRIM        1439      // steering center
#define THROTTLE_TRIM     1463      // zero throttle
#define THROTTLE_MIN      1463      // no going backwards
#define THROTTLE_MAX      1700      // set conservatively??? FIXME old was 1600
#define STEER_MIN         1235      // max right turn physically 1100
#define STEER_MAX         1635      // max left turn physically 1800
#define E_STOP_UPPER      1758      // outside of this, switch to manual control
#define E_STOP_LOWER      1158      // outside of this, switch to autonomous control

// other details
#define START_DELAY          0
#define CRUISE_VELOCITY  75000
#define STEER_VELOCITY   35000

/****************************************
 * Initialization                       *
 ****************************************/
// host communication
FastSerialPort0(Serial);    // FTDI on Serial0

// GPS
FastSerialPort1(Serial1);   // GPS on Serial1
AP_GPS_MTK gps(&Serial1);   // SkyLab SKM53, finicky setup required

// ADC
AP_ADC_ADS7844      adc;

// IMU
AP_IMU_Oilpan       imu(&adc, 0);

// DCM
AP_DCM              dcm(&imu, &gps);    // requires minor edit to libraries to compile

// Barometer
APM_BMP085_Class    barometer;

// Compass
AP_Compass_HMC5843  compass;

// Rangefinder
AP_RangeFinder_MaxsonarLV rangefinder;

// Velocity PID
PID                 vel_pid(NULL, 0.0, 0.0, 0.0, 0.0);

/****************************************
 * Variables                            *
 ****************************************/
// control variables
boolean         autonomous              = 0;    // 0 = manual control, 1 = automatic control
boolean         go_button               = 0;    // allows for delayed start
unsigned long   go_time;                        // ms
float           vel_target              = 0;    // counts/sec
float           vel_error               = 0;    // counts/sec
float           vel_scale               = 0.0001;  // velocity scale factor for PID loop
float           vel_control             = THROTTLE_TRIM;    // output of PID loop
float           heading_error           = 0;    // difference between waypoint heading and compass
float           steer_control           = STEER_TRIM;   // output of steering controller

// sensor variables
bool            GPS_fix                 = 0;    // GPS position fix status
struct Location current_loc;                    // current location (alt, lat, lng)
long            GPS_ground_speed;               // GPS ground speed (cm/sec)
float           GPS_ground_course;              // GPS ground course (deg)
int             GPS_hdop;                       // GPS horizontal dilution of precision (cm)
uint8_t         GPS_num_sats;                   // number of visible GPS satellites

float           compass_heading;                // compass heading (deg)

long            encoder                 = 0;    // encoder value (counts)
float           velocity                = 0.0;  // encoder velocity (counts/sec)

float           rf;                             // rangefinder value (cm)

// encoder lookup table, zeroes with spaces indicate invalid jumps
int8_t enc_lookup[] = {0,-1,1, 0,1,0, 0,-1,-1, 0,0,1, 0,1,-1,0};

// timing variables
unsigned long   begin_timestamp;                // begin time of first main loop
unsigned long   fast_loop_timestamp     = 0;    // begin time of fast loop
unsigned long   medium_loop_timestamp   = 0;    // begin time of medium loop
unsigned long   slow_loop_timestamp     = 0;    // begin time of slow loop
unsigned int    delta_ms_fast_loop;             // time since last fast loop was executed
unsigned int    delta_ms_medium_loop;           // time since last medium loop was executed
unsigned int    delta_ms_slow_loop;             // time since last slow loop was executed
unsigned long   fast_loop_counter       = 0;    // number of fast loops executed
unsigned long   medium_loop_counter     = 0;    // number of medium loops executed
unsigned long   slow_loop_counter       = 0;    // number of slow loops executed

float           G_Dt                    = 0.01; // integration time for gyros

// servo variables
uint16_t        ch_temp = STEER_TRIM;           // temporary RC channel variable

// navigation variables
struct wp_xy  current_loc_xy;                   // current location (x, y)
struct Location home_loc;                       // home location (lat, lng)
struct wp_xy  wp[50];                           // array of waypoints
int num_wp = 0;                                 // number of waypoints in course
int wp_i = 0;                                   // current waypoint index

float           old_target_distance;            // distance to last waypoint (cm)
float           target_distance;                // distance to next waypoint (cm)
float           target_heading;                 // heading to next wp from current position

/****************************************
 * Encoder Reading                      *
 ****************************************/
// interrupt routine on pin change
ISR(PCINT1_vect) {
    read_enc();
}

void read_enc() {
    // old encoder state byte
    static uint8_t old_enc;

    // store the old encoder state by shifting left
    old_enc <<= 2;

    // after this operation, the lower 4 bits of old_enc point to the appropriate element in enc_lookup
    // (A = PINJ0, B = PINJ1)
    old_enc |= PINJ & 0b00000011;

    // update the encoder counter, lookup index is lower 4 bits of old_enc
    encoder += enc_lookup[old_enc & 0x0F];
}

/****************************************
 * Setup                                *
 ****************************************/
void setup() {
    // host communication
    Serial.begin(FTDI_BAUDRATE);
    //Serial.println("RCbot host interface initialized...");

    // servos
    APM_RC.Init();
    APM_RC.OutputCh(ID_STEER, STEER_TRIM);
    APM_RC.OutputCh(ID_THROTTLE, THROTTLE_TRIM);
    //Serial.println("Servos initialized...");

    // GPS
    Serial1.begin(GPS_BAUDRATE);
    gps.init();
    //Serial.println("GPS sensor initialized...");

    // ADC
    adc.Init();

    // IMU
    imu.init(IMU::WARM_START); // FIXME, relies on prior good cold start

    // compass
    Wire.begin();
    compass.init();
    compass.set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_LEFT);
    compass.set_offsets(12.00,76.50,-388.00);
    compass.set_declination(deg2rad(declination));
    //Serial.println("Compass initialized...");

    // barometer
    barometer.Init();
    //Serial.println("Barometer initialized...");

    // rangefinder
    rangefinder.init(RF_PIN, &adc);
    //Serial.println("Rangefinder initialized...");

    // encoder
    // Port J 0-1 are the encoder inputs
    PORTJ &= 0b11111100;
    DDRJ  &= 0b11111100;
    // turn on pin change interrupt for PCINT23:16
    PCICR |= 0x02;
    // enable interrupts on PCINT9 and PCINT10
    PCMSK1 |= 0x06;
    //Serial.println("Encoder initialized...");

    // velocity PID, gains hardcoded!
    vel_pid.kP(1.0);
    vel_pid.kI(0.5);
    vel_pid.kD(0.5);
    vel_pid.imax(1);

    // LEDs
    pinMode(A_LED_PIN, OUTPUT);     // used to indicate autonomous mode
    pinMode(B_LED_PIN, OUTPUT);     // used to indicate GPS fix
    pinMode(C_LED_PIN, OUTPUT);     // currently unused

    // waypoints hardcoded, path #5
    num_wp = 6;
    wp[0].x = dlng2x((-105.2097629156277*10000000 - ref_lng)); // turn 1
    wp[0].y = dlat2y((40.06516511363822*10000000 - ref_lat));
    wp[1].x = dlng2x((-105.2097759310495*10000000 - ref_lng)); // pre-hoop
    wp[1].y = dlat2y((40.06488852554281*10000000 - ref_lat));
    wp[2].x = dlng2x((-105.2097869822649*10000000 - ref_lng)); // turn 2
    wp[2].y = dlat2y((40.06449614192299*10000000 - ref_lat));
    wp[3].x = dlng2x((-105.2104576802988*10000000 - ref_lng)); // turn 3
    wp[3].y = dlat2y((40.06449794385141*10000000 - ref_lat));
    wp[4].x = dlng2x((-105.2104600859605*10000000 - ref_lng)); // turn 4
    wp[4].y = dlat2y((40.06516566306915*10000000 - ref_lat));
    wp[5].x = dlng2x((-105.2099901736422*10000000 - ref_lng)); // post-finish line
    wp[5].y = dlat2y((40.06517189989115*10000000 - ref_lat));
}

/****************************************
 * Fast Loop                            *
 ****************************************/
// handles actuator control
void fast_loop() {
    // old encoder value
    static long old_encoder;        // encoder value from last time around
    static long store_encoder;      // used to capture current encoder value

    // handle "switch" input
    ch_temp = APM_RC.InputCh(ID_SAFETY);
    if (ch_temp < E_STOP_LOWER) {
        // turn autonomous control off
        autonomous = 0;
    }
    if (ch_temp > E_STOP_UPPER) {
        // turn autonomy on after delay
        go_button = 1;
        go_time = millis() + START_DELAY;
    }

    if (go_button) {
        // turn auto on
        if (millis() >= go_time) {
            go_button = 0;
            autonomous = 1;
            steer_control = STEER_TRIM;
            vel_pid.reset_I();
            vel_control = THROTTLE_TRIM;
            vel_target = CRUISE_VELOCITY;
        }
    }

    // GPS fix status check?
    if (GPS_fix != 1) {
        // turn autonomous control off
        autonomous = 0;
    }

    // update the DCM
    dcm.update_DCM(G_Dt);

    // record encoder value
    store_encoder = encoder;
    // calculate velocity
    velocity = (float)(store_encoder - old_encoder) / delta_ms_fast_loop * 1000.0;
    // store encoder value
    old_encoder = store_encoder;

    // do the velocity PID loop
    vel_error = vel_target - velocity;
    vel_control = vel_control + vel_pid.get_pid(vel_error, delta_ms_fast_loop, vel_scale);

    // do the servo control
    if (autonomous) {
        // servo control stuff goes here

        // steering safety
        if (steer_control > STEER_MAX) {
            steer_control = STEER_MAX;
        } else if (steer_control < STEER_MIN) {
            steer_control = STEER_MIN;
        }

        // throttle safety
        if (vel_control > THROTTLE_MAX) {
            vel_control = THROTTLE_MAX;
        }
        else if (vel_control < THROTTLE_MIN) {
            vel_control = THROTTLE_MIN;
        }

        // run to specified steering
        APM_RC.OutputCh(ID_STEER, steer_control);

        // run to the specified throttle
        APM_RC.OutputCh(ID_THROTTLE, vel_control);
    } else {
        // we are in manual control mode
        APM_RC.OutputCh(ID_PAN, PAN_TRIM);
        APM_RC.OutputCh(ID_STEER, APM_RC.InputCh(ID_STEER));
        APM_RC.OutputCh(ID_THROTTLE, APM_RC.InputCh(ID_THROTTLE));
    }
}

/****************************************
 * Medium Loop                          *
 ****************************************/
// handles vehicle vector control loop
// reads GPS and magnetometer
void medium_loop() {
    // check for autonomous
    if (autonomous) {
        digitalWrite(A_LED_PIN, HIGH);
    } else {
        digitalWrite(A_LED_PIN, LOW);
    }

    // check for GPS lock
    if (GPS_fix) {
        digitalWrite(B_LED_PIN, HIGH);
    } else {
        digitalWrite(B_LED_PIN, LOW);
    }

    // read GPS
    gps.update();
    if (gps.new_data) {
        // record the relevant GPS data
        GPS_fix             = gps.fix;              // GPS fix status boolean
        current_loc.lat     = gps.latitude;         // latitude * 10^7
        current_loc.lng     = gps.longitude;        // longitude * 10^7
        current_loc.alt     = gps.altitude;         // altitude (cm)
        GPS_ground_speed    = gps.ground_speed;     // ground speed (cm/sec)
        if (gps.ground_course > 18000) {
            GPS_ground_course = (gps.ground_course - 36000) / 100.0;
        } else {
            GPS_ground_course   = gps.ground_course / 100.0;    // ground course (deg)
        }
        GPS_hdop            = gps.hdop;             // hdop (cm)
        GPS_num_sats        = gps.num_sats;         // number of visible satellites
        // mark this GPS data as read
        gps.new_data = 0;
    }

    // read compass
    compass.read();
    // compensate for roll and pitch
    compass.calculate(dcm.roll,dcm.pitch);
    // record the relevant compass data
    compass_heading = rad2deg(compass.heading) + COMPASS_MOUNT_OFFSET;  // FIXME not right way to do mounting offset

    // read rangefinder
    rf = rangefinder.read();

    // calculate current xy position
    current_loc_xy.x = dlng2x((current_loc.lng-ref_lng));
    current_loc_xy.y = dlat2y((current_loc.lat-ref_lat));

    // calculate target distance
    target_distance = sqrt(pow((wp[wp_i].x-current_loc_xy.x),2) + pow((wp[wp_i].y-current_loc_xy.y),2));

    // check if we are done with waypoints
    if (wp_i >= num_wp) {
        autonomous = 0;
    }

    // check if we are close enough to slow down
    if (target_distance < 400) {
        vel_target = STEER_VELOCITY;
    }

    // check if we are far enough away to speed up
    if (wp_i > 0) {
        old_target_distance = sqrt(pow((wp[wp_i-1].x-current_loc_xy.x),2) + pow((wp[wp_i-1].y-current_loc_xy.y),2));
        if (old_target_distance > 400) {
            vel_target = CRUISE_VELOCITY;
        }
    }

    // check if we have achieved WP
    if (target_distance < 200) {
        wp_i++;
    }

    // calculate target heading
    target_heading = atan2((wp[wp_i].y-current_loc_xy.y),(wp[wp_i].x-current_loc_xy.x));
    // above is in radians and off by 90 deg, let's correct that, and also flip sign
    target_heading = -(rad2deg(target_heading)-90.0);
    // shift domain
    if (target_heading < -180.0) {
        target_heading = target_heading + 360.0;
    }
    if (target_heading > 180.0) {
        target_heading = target_heading - 360.0;
    }

    // do the steering control
    heading_error = target_heading - compass_heading;
    // shift domain
    if (heading_error > 180) {
        heading_error = heading_error - 360;
    } else if (heading_error < -180) {
        heading_error = heading_error + 360;
    }
    // scale the steering to heading error
    if (abs(heading_error) > 2) {       // 4deg deadband for compass noise
        steer_control = STEER_TRIM - (heading_error * 2.3);
    } else {
        steer_control = STEER_TRIM;
    }
}

/****************************************
 * Slow Loop                            *
 ****************************************/
void slow_loop() {
    //Serial.print("f_loops:");
    //Serial.println(fast_loop_counter);
    //Serial.print("m_loops:");
    //Serial.println(medium_loop_counter);
    //Serial.print("s_loops:");
    //Serial.println(slow_loop_counter);

    //Serial.print("enc: ");
    //Serial.println(encoder);
    //Serial.print("vel_t: ");
    //Serial.println(vel_target);
    //Serial.print("vel  : ");
    //Serial.println(velocity);
    //Serial.print("throt: ");
    //Serial.println(vel_control);

    // print target distance
    //Serial.print("targd: ");
    //Serial.println(target_distance);

    // print target heading
    //Serial.print("targh: ");
    //Serial.println(target_heading);

    // print compass heading
    //Serial.print("compass: ");
    //Serial.println(compass_heading);

    // print heading error
    //Serial.print("head error: ");
    //Serial.println(heading_error);

    // print steer control
    //Serial.print("str ctl: ");
    //Serial.println(steer_control);

    // print velocity target
    //Serial.print("vel targ: ");
    //Serial.println(vel_target);

    // print current velocity
    //Serial.print("velocity: ");
    //Serial.println(velocity);

    // print rangefinder value (cm)
    //Serial.print("range: ");
    //Serial.println(rf);

    // print steer input
    //Serial.print("str: ");
    //Serial.println(APM_RC.InputCh(ID_STEER));
}

/****************************************
 * Main Loop                            *
 ****************************************/
void loop() {
    // run the fast loop at 100Hz
    if (millis() - fast_loop_timestamp > 9) {
        delta_ms_fast_loop = millis() - fast_loop_timestamp;
        G_Dt = (float)delta_ms_fast_loop / 1000.f;
        fast_loop_timestamp = millis();
        fast_loop();
        fast_loop_counter++;
    }

    // run the medium loop at 10Hz
    if (millis() - medium_loop_timestamp > 99) {
        delta_ms_medium_loop = millis() - medium_loop_timestamp;
        medium_loop_timestamp = millis();
        medium_loop();
        medium_loop_counter++;
    }

    // run the slow loop at 1Hz
    if (millis() - slow_loop_timestamp > 999) {
        delta_ms_slow_loop = millis() - slow_loop_timestamp;
        slow_loop_timestamp = millis();
        slow_loop();
        slow_loop_counter++;
    }
}