Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // 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++;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement