Guest User

RCbot

a guest
Apr 26th, 2011
149
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

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×