Elisa3
|
00001 #include <avr\io.h> 00002 00003 /***************/ 00004 /*** VARIOUS ***/ 00005 /***************/ 00006 #ifndef RAD_2_DEG 00007 #define RAD_2_DEG 57.2957796 // conversion factor from radiant to degrees; 00008 #endif // use: degrees_value = radiant_value * RAD_2_DEG 00009 00010 #ifndef CALIBRATION_CYCLES 00011 #define CALIBRATION_CYCLES 16 // number of samples used for calibration 00012 #endif 00013 00014 #ifndef PAUSES // pauses based on adc base time counter "delayCounter" 00015 #define PAUSE_1_MSEC 9 // they are not perfectly precise (based on 104 us) 00016 #define PAUSE_10_MSEC 96 00017 #define PAUSE_100_MSEC 961 00018 #define PAUSE_200_MSEC 1923 00019 #define PAUSE_250_MSEC 2404 00020 #define PAUSE_300_MSEC 2885 00021 #define PAUSE_400_MSEC 3846 00022 #define PAUSE_500_MSEC 4807 00023 #define PAUSE_750_MSEC 7211 00024 #define PAUSE_1_SEC 9615 00025 #define PAUSE_1500_MSEC 14423 00026 #define PAUSE_2_SEC 19230 00027 #define PAUSE_4_SEC 38462 00028 #define PAUSE_5_SEC 48077 00029 #define PAUSE_6_SEC 57692 00030 #define PAUSE_10_SEC 96154 00031 #define PAUSE_15_SEC 144230 00032 #define PAUSE_20_SEC 192307 00033 #define PAUSE_30_SEC 288461 00034 #define PAUSE_40_SEC 384615 00035 #define PAUSE_60_SEC 576923 00036 #endif 00037 00038 #ifndef UART_BUFF_SIZE 00039 #define UART_BUFF_SIZE 256 00040 #endif 00041 00042 #define LINE_IN_THR 400 00043 #define LINE_OUT_THR 450 00044 00045 #define SPEED_STEP_DELAY 400 // based on adc interrupt that is about 104 us 00046 // speed updated every SPEED_STEP_FREQ*104 us untill desired 00047 // speed reached 00048 00049 /***********/ 00050 /*** NRF ***/ 00051 /***********/ 00052 #ifndef PAYLOAD_SIZE 00053 #define PAYLOAD_SIZE 13 // payload of the packet received from the computer 00054 #endif 00055 00056 /************************/ 00057 /*** SPEED CONTROLLER ***/ 00058 /************************/ 00059 #ifndef PERIOD_MOTORS_100HZ 00060 #define PERIOD_MOTORS_100HZ 40000 // old controller (elisa 2): we need a period time of 10 ms (100 Hz) => 0.01 * 4000000 = 40000 00061 #endif 00062 00063 #ifndef MAX_PWM 00064 #define MAX_PWM PERIOD_MOTORS_100HZ/100*60 // old controller (elisa 2): 60% of maximum 00065 #endif 00066 00067 #ifndef MAX_MOTORS_PWM 00068 #define MAX_MOTORS_PWM 1024 // max value for the pwm registers of the motors; 50% of maximum is then used actually 00069 #endif 00070 00071 // horizontal speed controller 00072 #ifndef P_HORIZONTAL 00073 #define P_HORIZONTAL 15 //25 // P parameter of the PID speed controller 00074 #endif 00075 00076 #ifndef I_HORIZONTAL 00077 #define I_HORIZONTAL 2 // I parameter of the PID speed controller 00078 #endif 00079 00080 #ifndef D_HORIZONTAL 00081 #define D_HORIZONTAL 3 // D parameter of the PID speed controller 00082 #endif 00083 00084 #ifndef I_LIMIT_HORIZONTAL 00085 #define I_LIMIT_HORIZONTAL 1600 // limit of the errors sum 00086 #endif 00087 00088 // vertical speed controller 00089 #ifndef P_VERTICAL 00090 #define P_VERTICAL 25 //10 00091 #endif 00092 00093 #ifndef I_VERTICAL 00094 #define I_VERTICAL 3 //3 00095 #endif 00096 00097 #ifndef D_VERTICAL 00098 #define D_VERTICAL 2 00099 #endif 00100 00101 #ifndef I_LIMIT_VERTICAL 00102 #define I_LIMIT_VERTICAL 1600 00103 #endif 00104 00105 #ifndef INIT_KFF 00106 #define INIT_KFF 25 // base feed forward for the vertical speed controller; 00107 #endif // used then for computing the actual feed forward based on 00108 // the robot vertical angle 00109 /****************/ 00110 /*** RGB LEDS ***/ 00111 /****************/ 00112 #ifndef MAX_LEDS_PWM 00113 #define MAX_LEDS_PWM 255 // max value for the pwm registers of the leds 00114 #endif 00115 00116 /*************************/ 00117 /*** IR REMOTE CONTROL ***/ 00118 /*************************/ 00119 #ifndef STEP_MOTORS 00120 #define STEP_MOTORS 2 // step used to increase/decrease the motors speed when 00121 #endif // receiving a command through TV remote. 00122 // speed (0..127) is expressed in 1/5 of mm/s (0..635 mm/s), thus 00123 // 2 means steps of 10 mm/s 00124 00125 /*********************/ 00126 /*** ACCELEROMETER ***/ 00127 /*********************/ 00128 #ifndef VERTICAL_THRESHOLD 00129 #define VERTICAL_THRESHOLD 20 // threshold value to swtich from horizontal to vertical plane 00130 #endif // (when Z > threshold then vertical plane) 00131 00132 #ifndef SAME_POS_NUM 00133 #define SAME_POS_NUM 200 // hysteresis value to pass from horizontal to vertical plane and 00134 #endif // viceversa. 00135 00136 #ifndef VERTICAL_POS 00137 #define VERTICAL_POS 0 // for easier code comprehension 00138 #endif 00139 00140 #ifndef HORIZONTAL_POS 00141 #define HORIZONTAL_POS 1 // for easier code comprehension 00142 #endif 00143 00144 #ifndef MMA7455L_ADDR 00145 #define MMA7455L_ADDR (0x1D<<1) // address to communicate with the MMA7455L 00146 #endif 00147 00148 #ifndef ADXL345_ADDR 00149 #define ADXL345_ADDR (0x53<<1) // address to communicate with the ADXL345 00150 #endif 00151 00152 #ifndef USE_MMAX7455L 00153 #define USE_MMAX7455L 0 // for easier code comprehension 00154 #endif 00155 00156 #ifndef USE_ADXL345 00157 #define USE_ADXL345 1 // for easier code comprehension 00158 #endif 00159 00160 #ifndef USE_NO_ACCEL 00161 #define USE_NO_ACCEL 2 // for easier code comprehension 00162 #endif 00163 00164 /*********************/ 00165 /*** HARDWARE DEFS ***/ 00166 /*********************/ 00167 #ifndef HW_REV 00168 #define HW_REV_3_0 0 00169 #define HW_REV_3_0_1 1 00170 #define HW_REV_3_1 2 00171 #endif 00172 00173 #ifndef REMOTE 00174 #define REMOTE (PINJ & _BV(PJ6))>>6 // tv remote pin state 00175 #endif 00176 00177 #ifndef SEL0 00178 #define SEL0 (PINC & _BV(PC0))>>0 // selector pins state 00179 #endif 00180 00181 #ifndef SEL1 00182 #define SEL1 (PINC & _BV(PC1))>>1 00183 #endif 00184 00185 #ifndef SEL2 00186 #define SEL2 (PINC & _BV(PC2))>>2 00187 #endif 00188 00189 #ifndef SEL3 00190 #define SEL3 (PINC & _BV(PC3))>>3 00191 #endif 00192 00193 #ifndef CHARGE_ON 00194 #define CHARGE_ON ((PINJ & _BV(PJ4))>>4) // charge_on pin state: 0 => robot not in charge; 1 => robot in charge 00195 #endif 00196 00197 #ifndef BUTTON0 00198 #define BUTTON0 ((PINJ & _BV(PJ5))>>5) // button pin state: 0 => button pressed; 1 => button not pressed 00199 #endif 00200 00201 #ifndef CHARGE_STAT 00202 #define CHARGE_STAT ((PINJ & _BV(PJ7))>>7) // charge_stat pin state: 0 => robot not charged completely; 1 => robot charged completely 00203 #endif 00204 00205 #ifndef SENS_ENABLE_OFF 00206 #define SENS_ENABLE_OFF (PORTC &= ~(1 << 6))// used for sampling the battery 00207 #endif 00208 00209 #ifndef SENS_ENABLE_ON 00210 #define SENS_ENABLE_ON (PORTC |= (1 << 6)) 00211 #endif 00212 00213 #ifndef LED_IR1_HIGH 00214 #define LED_IR1_HIGH (PORTC |= (1<<4)) // turn off backward IR 00215 #endif 00216 00217 #ifndef LED_IR1_LOW 00218 #define LED_IR1_LOW (PORTC &= ~(1<<4)) // turn on backward IR 00219 #endif 00220 00221 #ifndef LED_IR2_HIGH 00222 #define LED_IR2_HIGH (PORTC |= (1<<5)) // turn off both forward IRs 00223 #endif 00224 00225 #ifndef LED_IR2_LOW 00226 #define LED_IR2_LOW (PORTC &= ~(1<<5)) // turn on both forward IRs 00227 #endif 00228 00229 // the LED_X_ON/OFF macros works only if the leds port pins aren't 00230 // set in pwm mode 00231 #ifndef LED_RED_OFF 00232 #define LED_RED_OFF (PORTB |= (1<<5)) 00233 #endif 00234 00235 #ifndef LED_GREEN_OFF 00236 #define LED_GREEN_OFF (PORTB |= (1<<6)) 00237 #endif 00238 00239 #ifndef LED_BLUE_OFF 00240 #define LED_BLUE_OFF (PORTB |= (1<<7)) 00241 #endif 00242 00243 #ifndef LED_RED_ON 00244 #define LED_RED_ON (PORTB &= ~(1<<5)) 00245 #endif 00246 00247 #ifndef LED_GREEN_ON 00248 #define LED_GREEN_ON (PORTB &= ~(1<<6)) 00249 #endif 00250 00251 #ifndef LED_BLUE_ON 00252 #define LED_BLUE_ON (PORTB &= ~(1<<7)) 00253 #endif 00254 00255 // small green leds handling macros 00256 #ifndef GREEN_LEDS 00257 #define GREEN_LED0_ON (PORTL &= ~(1 << 0)) 00258 #define GREEN_LED1_ON (PORTL &= ~(1 << 1)) 00259 #define GREEN_LED2_ON (PORTL &= ~(1 << 2)) 00260 #define GREEN_LED3_ON (PORTG &= ~(1 << 3)) 00261 #define GREEN_LED4_ON (PORTL &= ~(1 << 4)) 00262 #define GREEN_LED5_ON (PORTL &= ~(1 << 5)) 00263 #define GREEN_LED6_ON (PORTL &= ~(1 << 6)) 00264 #define GREEN_LED7_ON (PORTL &= ~(1 << 7)) 00265 #define GREEN_LED0_OFF (PORTL |= (1 << 0)) 00266 #define GREEN_LED1_OFF (PORTL |= (1 << 1)) 00267 #define GREEN_LED2_OFF (PORTL |= (1 << 2)) 00268 #define GREEN_LED3_OFF (PORTG |= (1 << 3)) 00269 #define GREEN_LED4_OFF (PORTL |= (1 << 4)) 00270 #define GREEN_LED5_OFF (PORTL |= (1 << 5)) 00271 #define GREEN_LED6_OFF (PORTL |= (1 << 6)) 00272 #define GREEN_LED7_OFF (PORTL |= (1 << 7)) 00273 #endif 00274 00275 /***********/ 00276 /*** ADC ***/ 00277 /***********/ 00278 #ifndef ACTIVE_PHASE 00279 #define ACTIVE_PHASE 0 // for easier code comprehension: indicate the active phase of the motors pwm 00280 #endif 00281 00282 #ifndef PASSIVE_PHASE 00283 #define PASSIVE_PHASE 1 // for easier code comprehension: indicate the passive phase of the motors pwm 00284 #endif 00285 00286 #ifndef NO_PHASE 00287 #define NO_PHASE 2 // currently no more used (when pwm = 0) 00288 #endif 00289 00290 #ifndef SAVE_TO_PROX 00291 #define SAVE_TO_PROX 0 // for easier code comprehension: indicate that the value currently sampled has 00292 #endif // to be saved in the proximity array 00293 00294 #ifndef SAVE_TO_RIGHT_MOTOR_CURRENT 00295 #define SAVE_TO_RIGHT_MOTOR_CURRENT 1 // for easier code comprehension: current sample is current consumption for right motor 00296 #endif 00297 00298 #ifndef SAVE_TO_RIGHT_MOTOR_VEL 00299 #define SAVE_TO_RIGHT_MOTOR_VEL 2 // for easier code comprehension: current sample is velocity for right motor 00300 #endif 00301 00302 #ifndef SAVE_TO_LEFT_MOTOR_CURRENT 00303 #define SAVE_TO_LEFT_MOTOR_CURRENT 3 // for easier code comprehension: current sample is current consumption for left motor 00304 #endif 00305 00306 #ifndef SAVE_TO_LEFT_MOTOR_VEL 00307 #define SAVE_TO_LEFT_MOTOR_VEL 4 // for easier code comprehension: current sample is velocity for left motor 00308 #endif 00309 00310 #ifndef SKIP_SAMPLE 00311 #define SKIP_SAMPLE 5 // currently no more needed 00312 #endif 00313 00314 #ifndef SAVE_TO_PROX_IRCOMM 00315 #define SAVE_TO_PROX_IRCOMM 6 00316 #endif 00317 00318 /***************/ 00319 /*** SENSORS ***/ 00320 /***************/ 00321 #ifndef PROX0_AMBIENT // array indices for the sensors for easier code comprehension 00322 #define PROX0_AMBIENT 0 00323 #define PROX0_REFLECTED 1 00324 #define PROX1_AMBIENT 2 00325 #define PROX1_REFLECTED 3 00326 #define PROX2_AMBIENT 4 00327 #define PROX2_REFLECTED 5 00328 #define PROX3_AMBIENT 6 00329 #define PROX3_REFLECTED 7 00330 #define PROX4_AMBIENT 8 00331 #define PROX4_REFLECTED 9 00332 #define PROX5_AMBIENT 10 00333 #define PROX5_REFLECTED 11 00334 #define PROX6_AMBIENT 12 00335 #define PROX6_REFLECTED 13 00336 #define PROX7_AMBIENT 14 00337 #define PROX7_REFLECTED 15 00338 #define GROUND0_AMBIENT 16 00339 #define GROUND0_REFLECTED 17 00340 #define GROUND1_AMBIENT 18 00341 #define GROUND1_REFLECTED 19 00342 #define GROUND2_AMBIENT 20 00343 #define GROUND2_REFLECTED 21 00344 #define GROUND3_AMBIENT 22 00345 #define GROUND3_REFLECTED 23 00346 #endif 00347 00348 /***********************/ 00349 /*** CLIFF AVOIDANCE ***/ 00350 /***********************/ 00351 #ifndef CLIFF_THR 00352 #define CLIFF_THR 420 // ground sensors threshold for detecting a cliff independently of the surface 00353 #endif // since the ground sensors are calibrated to 512 00354 00355 /**************************/ 00356 /*** OBSTACLE AVOIDANCE ***/ 00357 /**************************/ 00358 #ifndef PHASE1 // range values used for linearization of the proximity; this values define 00359 #define PHASE1 60 // the passage from one to the next linear function used to approximate and 00360 #endif // linearize the proximity values 00361 #ifndef PHASE2 00362 #define PHASE2 120 00363 #endif 00364 #ifndef PHASE3 00365 #define PHASE3 180 00366 #endif 00367 #ifndef NOISE_THR // define the value under which the proximity is considered to be noise 00368 #define NOISE_THR 5 00369 #endif 00370 00371 /****************/ 00372 /*** ODOMETRY ***/ 00373 /****************/ 00374 #define WHEEL_DIST 39.5 // mm 00375 // the encoders values depends on desired speed: enc = enc + meas_speed*k, where k=function(desired_speed)= a*desired_speed + b 00376 // the following values represent the "b" (offset) and "a" (slope) of the function 00377 #define LEFT_ENC_OFFSET 80.8 // these values are obtained from field test 00378 #define RIGHT_ENC_OFFSET 78.6 00379 #define ENC_SLOPE 0.384 00380 00381 #define LEFT_WHEEL_FW_SC 0 // forward, speed control enabled 00382 #define RIGHT_WHEEL_FW_SC 1 00383 #define LEFT_WHEEL_BW_SC 2 // backward, speed control enabled 00384 #define RIGHT_WHEEL_BW_SC 3 00385 #define DISTANCE_MM 248.18 // distance traveled during calibration => WHEEL_DIST*2*PI 00386 #define INDEX_STEP 5 //14 00387 #define BYTE_TO_MM_S 5 00388 #define CALIBRATION_SAMPLES 9 00389 #define CALIB_CHECK_ADDRESS 3946 00390 #define CALIB_DATA_START_ADDR 3948 00391 #define HYSTERESIS_THR 75 00392 00393 /***************/ 00394 /*** IR COMM ***/ 00395 /***************/ 00396 #define IRCOMM_MODE_SENSORS_SAMPLING 0 00397 #define IRCOMM_MODE_RECEIVE 1 00398 #define IRCOMM_MODE_TRANSMIT 2 00399 //#define IRCOMM_MODE_RECEIVE_ONLY 3 00400 //#define IRCOMM_MODE_TRANSMIT_ONLY 4 00401 #define IRCOMM_SAMPLING_WINDOW 20 00402 00403 00404 // reception 00405 #define IRCOMM_RX_IDLE_STATE 1 00406 #define IRCOMM_RX_MAX_SENSOR_STATE 2 00407 #define IRCOMM_RX_DETECT_START_BIT_STATE 3 00408 #define IRCOMM_RX_SYNC_SIGNAL 4 00409 #define IRCOMM_RX_WAITING_BIT 5 00410 #define IRCOMM_RX_READ_BIT 6 00411 #define IRCOMM_RX_CHECK_CRC 7 00412 #define IRCOMM_RX_DEBUG 8 00413 #define IRCOMM_RX_STOP 9 00414 00415 #define IRCOMM_DETECTION_AMPLITUDE_THR 80 //20 00416 #define IRCOMM_START_BIT_MIN_SWITCH_COUNT 2 00417 00418 // transmission 00419 #define IRCOMM_TX_IDLE_STATE 1 // wait the user data to be sent 00420 #define IRCOMM_TX_COMPUTE_TIMINGS 2 00421 #define IRCOMM_TX_TRANSMIT_BIT 3 00422 #define IRCOMM_TX_PREPARE_TRANSMISSION 5 00423 00424 #define IRCOMM_TX_ADC_IDLE 0 00425 #define IRCOMM_TX_ADC_TURN_OFF_SENSORS 1 00426 #define IRCOMM_TX_ADC_WAIT_PREPARATION 2 00427 #define IRCOMM_TX_ADC_TRANSMISSION_SEQ1 3 // we need two state for byte transmission to change the channel sampled 00428 #define IRCOMM_TX_ADC_TRANSMISSION_SEQ2 4 // related to motors 00429 00430 #define IRCOMM_BIT_START1_SWITCH_COUNT 2 00431 #define IRCOMM_BIT_START2_SWITCH_COUNT 1 00432 #define IRCOMM_BIT0_SWITCH_COUNT 8 00433 #define IRCOMM_BIT1_SWITCH_COUNT 4 00434 #define IRCOMM_BIT_START1_DURATION 120 00435 #define IRCOMM_BIT_START2_DURATION 240 00436 #define IRCOMM_BIT0_DURATION 30 // based on adc isr of 104 us 00437 #define IRCOMM_BIT1_DURATOIN 60 // based on adc isr of 104 us 00438 00439 // debug 00440 #define DEBUG_MAX_SENSOR_STATE 0 00441 #define DEBUG_ALL_SENSORS 0 00442 #define DEBUG_MAX_SENSOR 0 00443 #define DEBUG_START_BIT_STATE 0 00444 #define DEBUG_READ_BIT 0 00445 #define DEBUG_BYTE_RECEPTION 0 00446 #define DEBUG_BYTE_SEQUENCE 0 00447 00448