daily pastebin goal
85%
SHARE
TWEET

RCbot

a guest Apr 26th, 2011 100 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // RCbot
  2. // this code is provided "as is"
  3.  
  4. /****************************************
  5.  * Setup                                *
  6.  ****************************************/
  7. // AVR Runtime
  8. #include <avr/io.h>
  9. #include <avr/eeprom.h>
  10. #include <avr/pgmspace.h>
  11. #include <math.h>
  12.  
  13. // APM Libraries
  14. #include <FastSerial.h>
  15. #include <AP_Common.h>
  16. #include <APM_RC.h>             // ArduPilotMega RC library
  17. #include <AP_GPS.h>             // ArduPilot GPS library
  18. #include <Wire.h>               // Arduino I2C library
  19. #include <DataFlash.h>          // ArduPilotMega flash memory library
  20. #include <AP_ADC.h>             // ArduPilotMega analog to digital converter library
  21. #include <APM_BMP085.h>         // ArduPilotMega BMP085 (barometer) library
  22. #include <AP_Compass.h>         // ArduPilotMega magnetometer library
  23. #include <AP_Math.h>            // ArduPilotMega vector/matrix math library
  24. #include <AP_IMU.h>             // ArduPilotMega IMU library
  25. #include <AP_DCM.h>             // ArduPilotMega DCM library
  26. #include <PID.h>                // PID library
  27. #include <AP_RangeFinder.h>     // Rangefinder library
  28.  
  29. // Additional Data Types
  30. struct wp_xy {
  31.     int        id;              // waypoint ID
  32.     int32_t     x;              // x-coordinate relative to reference (cm)
  33.     int32_t     y;              // y-coordinate relative to reference (cm)
  34. };
  35.  
  36. // Math Constants
  37. #define deg2rad(deg) (deg*0.01745329252)  // *pi/180
  38. #define rad2deg(rad) (rad*57.2957795131)  // *180/pi
  39.  
  40. // Configuration
  41. #define RF_PIN            1007      // rangefinder analog input pin (pitot tube input)
  42. #define ENC_A_PIN           15      // aka PJ0/PCINT9 (telemetry port)
  43. #define ENC_B_PIN           14      // aka PJ1/PCINT10 (telemetry port)
  44.  
  45. #define A_LED_PIN           37
  46. #define B_LED_PIN           36
  47. #define C_LED_PIN           35
  48.  
  49. #define FTDI_BAUDRATE   115200      // FTDI serial interface baudrate
  50. #define GPS_BAUDRATE     38400      // GPS serial interface baudrate
  51.  
  52. #define ID_PAN               0      // Channel 0 is rangefinder pan (output)
  53. #define ID_SAFETY            1      // Channel 2 is safety (input)
  54. #define ID_STEER             1      // Channel 1 is steering
  55. #define ID_THROTTLE          2      // Channel 2 is throttle
  56.  
  57. /****************************************
  58.  * Parameters and Variables             *
  59.  ****************************************/
  60. // coordinate parameters (PASADENA)
  61. //#define ref_lat      341300000          // reference latitude * 10^7
  62. //#define ref_lng    -1181400000          // reference longitude * 10^7
  63. //#define dlng2x(dlng) (dlng*0.92234300)   // delta longitude (deg * 10^7) to x coordinate (cm)
  64. //#define dlat2y(dlat) (dlat*1.10924946)  // delta latitude (deg * 10^7) to y coordinate (cm)
  65. //#define declination 12.617              // in degrees
  66. // coordinate parameters (BOULDER)
  67. #define ref_lat      400648660          // reference latitude * 10^7
  68. #define ref_lng    -1052100770          // reference longitude * 10^7
  69. #define dlng2x(dlng) (dlng*0.85313000)   // delta longitude (deg) to x coordinate (cm)
  70. #define dlat2y(dlat) (dlat*1.11035939)  // delta latitude (deg) to y coordinate (cm)
  71. #define declination 9.083               // in degrees
  72.  
  73. #define COMPASS_MOUNT_OFFSET 3.5          // degrees to add to compensate for mounting offset    
  74.  
  75. // servo details
  76. #define PAN_TRIM          1500
  77. #define STEER_TRIM        1439      // steering center
  78. #define THROTTLE_TRIM     1463      // zero throttle
  79. #define THROTTLE_MIN      1463      // no going backwards
  80. #define THROTTLE_MAX      1700      // set conservatively??? FIXME old was 1600
  81. #define STEER_MIN         1235      // max right turn physically 1100
  82. #define STEER_MAX         1635      // max left turn physically 1800
  83. #define E_STOP_UPPER      1758      // outside of this, switch to manual control
  84. #define E_STOP_LOWER      1158      // outside of this, switch to autonomous control
  85.  
  86. // other details
  87. #define START_DELAY          0
  88. #define CRUISE_VELOCITY  75000
  89. #define STEER_VELOCITY   35000    
  90.  
  91. /****************************************
  92.  * Initialization                       *
  93.  ****************************************/
  94. // host communication
  95. FastSerialPort0(Serial);    // FTDI on Serial0
  96.  
  97. // GPS
  98. FastSerialPort1(Serial1);   // GPS on Serial1
  99. AP_GPS_MTK gps(&Serial1);   // SkyLab SKM53, finicky setup required
  100.  
  101. // ADC
  102. AP_ADC_ADS7844      adc;
  103.  
  104. // IMU
  105. AP_IMU_Oilpan       imu(&adc, 0);
  106.  
  107. // DCM
  108. AP_DCM              dcm(&imu, &gps);    // requires minor edit to libraries to compile
  109.  
  110. // Barometer
  111. APM_BMP085_Class    barometer;
  112.  
  113. // Compass
  114. AP_Compass_HMC5843  compass;
  115.  
  116. // Rangefinder
  117. AP_RangeFinder_MaxsonarLV rangefinder;
  118.  
  119. // Velocity PID
  120. PID                 vel_pid(NULL, 0.0, 0.0, 0.0, 0.0);
  121.  
  122. /****************************************
  123.  * Variables                            *
  124.  ****************************************/
  125. // control variables
  126. boolean         autonomous              = 0;    // 0 = manual control, 1 = automatic control
  127. boolean         go_button               = 0;    // allows for delayed start
  128. unsigned long   go_time;                        // ms    
  129. float           vel_target              = 0;    // counts/sec
  130. float           vel_error               = 0;    // counts/sec
  131. float           vel_scale               = 0.0001;  // velocity scale factor for PID loop
  132. float           vel_control             = THROTTLE_TRIM;    // output of PID loop
  133. float           heading_error           = 0;    // difference between waypoint heading and compass
  134. float           steer_control           = STEER_TRIM;   // output of steering controller
  135.  
  136. // sensor variables
  137. bool            GPS_fix                 = 0;    // GPS position fix status
  138. struct Location current_loc;                    // current location (alt, lat, lng)
  139. long            GPS_ground_speed;               // GPS ground speed (cm/sec)
  140. float           GPS_ground_course;              // GPS ground course (deg)
  141. int             GPS_hdop;                       // GPS horizontal dilution of precision (cm)
  142. uint8_t         GPS_num_sats;                   // number of visible GPS satellites
  143.  
  144. float           compass_heading;                // compass heading (deg)
  145.  
  146. long            encoder                 = 0;    // encoder value (counts)
  147. float           velocity                = 0.0;  // encoder velocity (counts/sec)
  148.  
  149. float           rf;                             // rangefinder value (cm)
  150.  
  151. // encoder lookup table, zeroes with spaces indicate invalid jumps
  152. int8_t enc_lookup[] = {0,-1,1, 0,1,0, 0,-1,-1, 0,0,1, 0,1,-1,0};
  153.  
  154. // timing variables
  155. unsigned long   begin_timestamp;                // begin time of first main loop
  156. unsigned long   fast_loop_timestamp     = 0;    // begin time of fast loop
  157. unsigned long   medium_loop_timestamp   = 0;    // begin time of medium loop
  158. unsigned long   slow_loop_timestamp     = 0;    // begin time of slow loop
  159. unsigned int    delta_ms_fast_loop;             // time since last fast loop was executed
  160. unsigned int    delta_ms_medium_loop;           // time since last medium loop was executed
  161. unsigned int    delta_ms_slow_loop;             // time since last slow loop was executed
  162. unsigned long   fast_loop_counter       = 0;    // number of fast loops executed
  163. unsigned long   medium_loop_counter     = 0;    // number of medium loops executed
  164. unsigned long   slow_loop_counter       = 0;    // number of slow loops executed
  165.  
  166. float           G_Dt                            = 0.01; // integration time for gyros
  167.  
  168. // servo variables
  169. uint16_t        ch_temp = STEER_TRIM;           // temporary RC channel variable
  170.  
  171. // navigation variables
  172. struct wp_xy  current_loc_xy;                   // current location (x, y)
  173. struct Location home_loc;                       // home location (lat, lng)
  174. struct wp_xy  wp[50];                           // array of waypoints
  175. int num_wp = 0;                                 // number of waypoints in course
  176. int wp_i = 0;                                   // current waypoint index
  177.  
  178. float           old_target_distance;            // distance to last waypoint (cm)
  179. float           target_distance;                // distance to next waypoint (cm)
  180. float           target_heading;                 // heading to next wp from current position
  181.  
  182. /****************************************
  183.  * Encoder Reading                      *
  184.  ****************************************/
  185. // interrupt routine on pin change
  186. ISR(PCINT1_vect) {
  187.     read_enc();
  188. }
  189.  
  190. void read_enc() {
  191.     // old encoder state byte
  192.     static uint8_t old_enc;
  193.    
  194.     // store the old encoder state by shifting left
  195.     old_enc <<= 2;
  196.    
  197.     // after this operation, the lower 4 bits of old_enc point to the appropriate element in enc_lookup
  198.     // (A = PINJ0, B = PINJ1)
  199.     old_enc |= PINJ & 0b00000011;
  200.    
  201.     // update the encoder counter, lookup index is lower 4 bits of old_enc
  202.     encoder += enc_lookup[old_enc & 0x0F];
  203. }
  204.  
  205. /****************************************
  206.  * Setup                                *
  207.  ****************************************/
  208. void setup() {
  209.     // host communication
  210.     Serial.begin(FTDI_BAUDRATE);
  211.     //Serial.println("RCbot host interface initialized...");
  212.    
  213.     // servos
  214.     APM_RC.Init();
  215.     APM_RC.OutputCh(ID_STEER, STEER_TRIM);
  216.     APM_RC.OutputCh(ID_THROTTLE, THROTTLE_TRIM);
  217.     //Serial.println("Servos initialized...");
  218.    
  219.     // GPS
  220.     Serial1.begin(GPS_BAUDRATE);
  221.     gps.init();
  222.     //Serial.println("GPS sensor initialized...");
  223.    
  224.     // ADC
  225.     adc.Init();
  226.    
  227.     // IMU
  228.     imu.init(IMU::WARM_START); // FIXME, relies on prior good cold start
  229.    
  230.     // compass
  231.     Wire.begin();
  232.     compass.init();
  233.     compass.set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_LEFT);
  234.     compass.set_offsets(12.00,76.50,-388.00);              
  235.     compass.set_declination(deg2rad(declination));
  236.     //Serial.println("Compass initialized...");
  237.    
  238.     // barometer
  239.     barometer.Init();
  240.     //Serial.println("Barometer initialized...");
  241.    
  242.     // rangefinder
  243.     rangefinder.init(RF_PIN, &adc);
  244.     //Serial.println("Rangefinder initialized...");
  245.    
  246.     // encoder
  247.     // Port J 0-1 are the encoder inputs
  248.     PORTJ &= 0b11111100;
  249.     DDRJ  &= 0b11111100;
  250.     // turn on pin change interrupt for PCINT23:16
  251.     PCICR |= 0x02;
  252.     // enable interrupts on PCINT9 and PCINT10
  253.     PCMSK1 |= 0x06;
  254.     //Serial.println("Encoder initialized...");
  255.    
  256.     // velocity PID, gains hardcoded!
  257.     vel_pid.kP(1.0);
  258.     vel_pid.kI(0.5);
  259.     vel_pid.kD(0.5);
  260.     vel_pid.imax(1);
  261.    
  262.     // LEDs
  263.     pinMode(A_LED_PIN, OUTPUT);     // used to indicate autonomous mode
  264.     pinMode(B_LED_PIN, OUTPUT);     // used to indicate GPS fix
  265.     pinMode(C_LED_PIN, OUTPUT);     // currently unused
  266.    
  267.     // waypoints hardcoded, path #5
  268.     num_wp = 6;
  269.     wp[0].x = dlng2x((-105.2097629156277*10000000 - ref_lng)); // turn 1
  270.     wp[0].y = dlat2y((40.06516511363822*10000000 - ref_lat));
  271.     wp[1].x = dlng2x((-105.2097759310495*10000000 - ref_lng)); // pre-hoop
  272.     wp[1].y = dlat2y((40.06488852554281*10000000 - ref_lat));
  273.     wp[2].x = dlng2x((-105.2097869822649*10000000 - ref_lng)); // turn 2
  274.     wp[2].y = dlat2y((40.06449614192299*10000000 - ref_lat));
  275.     wp[3].x = dlng2x((-105.2104576802988*10000000 - ref_lng)); // turn 3
  276.     wp[3].y = dlat2y((40.06449794385141*10000000 - ref_lat));
  277.     wp[4].x = dlng2x((-105.2104600859605*10000000 - ref_lng)); // turn 4
  278.     wp[4].y = dlat2y((40.06516566306915*10000000 - ref_lat));
  279.     wp[5].x = dlng2x((-105.2099901736422*10000000 - ref_lng)); // post-finish line
  280.     wp[5].y = dlat2y((40.06517189989115*10000000 - ref_lat));
  281. }
  282.  
  283. /****************************************
  284.  * Fast Loop                            *
  285.  ****************************************/
  286. // handles actuator control
  287. void fast_loop() {
  288.     // old encoder value
  289.     static long old_encoder;        // encoder value from last time around
  290.     static long store_encoder;      // used to capture current encoder value
  291.  
  292.     // handle "switch" input
  293.     ch_temp = APM_RC.InputCh(ID_SAFETY);
  294.     if (ch_temp < E_STOP_LOWER) {
  295.         // turn autonomous control off
  296.         autonomous = 0;
  297.     }
  298.     if (ch_temp > E_STOP_UPPER) {
  299.         // turn autonomy on after delay
  300.         go_button = 1;
  301.         go_time = millis() + START_DELAY;
  302.     }
  303.    
  304.     if (go_button) {
  305.         // turn auto on
  306.         if (millis() >= go_time) {
  307.             go_button = 0;
  308.             autonomous = 1;
  309.             steer_control = STEER_TRIM;
  310.             vel_pid.reset_I();
  311.             vel_control = THROTTLE_TRIM;
  312.             vel_target = CRUISE_VELOCITY;
  313.         }
  314.     }
  315.    
  316.     // GPS fix status check?
  317.     if (GPS_fix != 1) {
  318.         // turn autonomous control off
  319.         autonomous = 0;
  320.     }
  321.    
  322.     // update the DCM
  323.     dcm.update_DCM(G_Dt);
  324.    
  325.     // record encoder value
  326.     store_encoder = encoder;
  327.     // calculate velocity
  328.     velocity = (float)(store_encoder - old_encoder) / delta_ms_fast_loop * 1000.0;
  329.     // store encoder value
  330.     old_encoder = store_encoder;
  331.    
  332.     // do the velocity PID loop
  333.     vel_error = vel_target - velocity;
  334.     vel_control = vel_control + vel_pid.get_pid(vel_error, delta_ms_fast_loop, vel_scale);
  335.    
  336.     // do the servo control
  337.     if (autonomous) {
  338.         // servo control stuff goes here
  339.        
  340.         // steering safety
  341.         if (steer_control > STEER_MAX) {
  342.             steer_control = STEER_MAX;
  343.         } else if (steer_control < STEER_MIN) {
  344.             steer_control = STEER_MIN;
  345.         }
  346.        
  347.         // throttle safety
  348.         if (vel_control > THROTTLE_MAX) {
  349.             vel_control = THROTTLE_MAX;
  350.         }
  351.         else if (vel_control < THROTTLE_MIN) {
  352.             vel_control = THROTTLE_MIN;
  353.         }
  354.        
  355.         // run to specified steering
  356.         APM_RC.OutputCh(ID_STEER, steer_control);
  357.        
  358.         // run to the specified throttle
  359.         APM_RC.OutputCh(ID_THROTTLE, vel_control);
  360.     } else {
  361.         // we are in manual control mode
  362.         APM_RC.OutputCh(ID_PAN, PAN_TRIM);
  363.         APM_RC.OutputCh(ID_STEER, APM_RC.InputCh(ID_STEER));
  364.         APM_RC.OutputCh(ID_THROTTLE, APM_RC.InputCh(ID_THROTTLE));
  365.     }
  366. }
  367.  
  368. /****************************************
  369.  * Medium Loop                          *
  370.  ****************************************/
  371. // handles vehicle vector control loop
  372. // reads GPS and magnetometer
  373. void medium_loop() {
  374.     // check for autonomous
  375.     if (autonomous) {
  376.         digitalWrite(A_LED_PIN, HIGH);
  377.     } else {
  378.         digitalWrite(A_LED_PIN, LOW);
  379.     }
  380.    
  381.     // check for GPS lock
  382.     if (GPS_fix) {
  383.         digitalWrite(B_LED_PIN, HIGH);
  384.     } else {
  385.         digitalWrite(B_LED_PIN, LOW);
  386.     }
  387.    
  388.     // read GPS
  389.     gps.update();
  390.     if (gps.new_data) {
  391.         // record the relevant GPS data
  392.         GPS_fix             = gps.fix;              // GPS fix status boolean
  393.         current_loc.lat     = gps.latitude;         // latitude * 10^7
  394.         current_loc.lng     = gps.longitude;        // longitude * 10^7
  395.         current_loc.alt     = gps.altitude;         // altitude (cm)
  396.         GPS_ground_speed    = gps.ground_speed;     // ground speed (cm/sec)
  397.         if (gps.ground_course > 18000) {
  398.             GPS_ground_course = (gps.ground_course - 36000) / 100.0;
  399.         } else {
  400.             GPS_ground_course   = gps.ground_course / 100.0;    // ground course (deg)
  401.         }
  402.         GPS_hdop            = gps.hdop;             // hdop (cm)
  403.         GPS_num_sats        = gps.num_sats;         // number of visible satellites
  404.         // mark this GPS data as read
  405.         gps.new_data = 0;
  406.     }
  407.    
  408.     // read compass
  409.     compass.read();
  410.     // compensate for roll and pitch
  411.     compass.calculate(dcm.roll,dcm.pitch);
  412.     // record the relevant compass data
  413.     compass_heading = rad2deg(compass.heading) + COMPASS_MOUNT_OFFSET;  // FIXME not right way to do mounting offset
  414.    
  415.     // read rangefinder
  416.     rf = rangefinder.read();
  417.    
  418.     // calculate current xy position
  419.     current_loc_xy.x = dlng2x((current_loc.lng-ref_lng));
  420.     current_loc_xy.y = dlat2y((current_loc.lat-ref_lat));
  421.    
  422.     // calculate target distance
  423.     target_distance = sqrt(pow((wp[wp_i].x-current_loc_xy.x),2) + pow((wp[wp_i].y-current_loc_xy.y),2));
  424.    
  425.     // check if we are done with waypoints
  426.     if (wp_i >= num_wp) {
  427.         autonomous = 0;
  428.     }
  429.    
  430.     // check if we are close enough to slow down
  431.     if (target_distance < 400) {
  432.         vel_target = STEER_VELOCITY;
  433.     }
  434.    
  435.     // check if we are far enough away to speed up
  436.     if (wp_i > 0) {
  437.         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));
  438.         if (old_target_distance > 400) {
  439.             vel_target = CRUISE_VELOCITY;
  440.         }
  441.     }
  442.    
  443.     // check if we have achieved WP
  444.     if (target_distance < 200) {
  445.         wp_i++;
  446.     }
  447.    
  448.     // calculate target heading
  449.     target_heading = atan2((wp[wp_i].y-current_loc_xy.y),(wp[wp_i].x-current_loc_xy.x));
  450.     // above is in radians and off by 90 deg, let's correct that, and also flip sign
  451.     target_heading = -(rad2deg(target_heading)-90.0);
  452.     // shift domain
  453.     if (target_heading < -180.0) {
  454.         target_heading = target_heading + 360.0;
  455.     }
  456.     if (target_heading > 180.0) {
  457.         target_heading = target_heading - 360.0;
  458.     }
  459.  
  460.     // do the steering control
  461.     heading_error = target_heading - compass_heading;
  462.     // shift domain
  463.     if (heading_error > 180) {
  464.         heading_error = heading_error - 360;
  465.     } else if (heading_error < -180) {
  466.         heading_error = heading_error + 360;
  467.     }
  468.     // scale the steering to heading error
  469.     if (abs(heading_error) > 2) {       // 4deg deadband for compass noise
  470.         steer_control = STEER_TRIM - (heading_error * 2.3);
  471.     } else {
  472.         steer_control = STEER_TRIM;
  473.     }
  474. }
  475.  
  476. /****************************************
  477.  * Slow Loop                            *
  478.  ****************************************/
  479. void slow_loop() {
  480.     //Serial.print("f_loops:");
  481.     //Serial.println(fast_loop_counter);
  482.     //Serial.print("m_loops:");
  483.     //Serial.println(medium_loop_counter);
  484.     //Serial.print("s_loops:");
  485.     //Serial.println(slow_loop_counter);
  486.    
  487.     //Serial.print("enc: ");
  488.     //Serial.println(encoder);
  489.     //Serial.print("vel_t: ");
  490.     //Serial.println(vel_target);
  491.     //Serial.print("vel  : ");
  492.     //Serial.println(velocity);
  493.     //Serial.print("throt: ");
  494.     //Serial.println(vel_control);
  495.    
  496.     // print target distance
  497.     //Serial.print("targd: ");
  498.     //Serial.println(target_distance);
  499.    
  500.     // print target heading
  501.     //Serial.print("targh: ");
  502.     //Serial.println(target_heading);
  503.    
  504.     // print compass heading
  505.     //Serial.print("compass: ");
  506.     //Serial.println(compass_heading);
  507.    
  508.     // print heading error
  509.     //Serial.print("head error: ");
  510.     //Serial.println(heading_error);
  511.    
  512.     // print steer control
  513.     //Serial.print("str ctl: ");
  514.     //Serial.println(steer_control);
  515.    
  516.     // print velocity target
  517.     //Serial.print("vel targ: ");
  518.     //Serial.println(vel_target);
  519.    
  520.     // print current velocity
  521.     //Serial.print("velocity: ");
  522.     //Serial.println(velocity);
  523.    
  524.     // print rangefinder value (cm)
  525.     //Serial.print("range: ");
  526.     //Serial.println(rf);
  527.    
  528.     // print steer input
  529.     //Serial.print("str: ");
  530.     //Serial.println(APM_RC.InputCh(ID_STEER));
  531. }
  532.  
  533. /****************************************
  534.  * Main Loop                            *
  535.  ****************************************/
  536. void loop() {
  537.     // run the fast loop at 100Hz
  538.     if (millis() - fast_loop_timestamp > 9) {
  539.         delta_ms_fast_loop = millis() - fast_loop_timestamp;
  540.         G_Dt = (float)delta_ms_fast_loop / 1000.f;
  541.         fast_loop_timestamp = millis();
  542.         fast_loop();
  543.         fast_loop_counter++;
  544.     }
  545.    
  546.     // run the medium loop at 10Hz
  547.     if (millis() - medium_loop_timestamp > 99) {
  548.         delta_ms_medium_loop = millis() - medium_loop_timestamp;
  549.         medium_loop_timestamp = millis();
  550.         medium_loop();
  551.         medium_loop_counter++;
  552.     }
  553.        
  554.     // run the slow loop at 1Hz
  555.     if (millis() - slow_loop_timestamp > 999) {
  556.         delta_ms_slow_loop = millis() - slow_loop_timestamp;
  557.         slow_loop_timestamp = millis();
  558.         slow_loop();
  559.         slow_loop_counter++;
  560.     }
  561. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top