00001 #include "pwm_compass.h"
00002
00003
00004
00005
00006 static short compass_pwm_pin, compass_calibrate_pin;
00007 static int pwm_delay_timer = 0;
00008 static const int PWM_DELAY_DURATION = 1000;
00009 static int compass_heading = 0;
00010
00011
00012 int initializeCompass(int pwm_pin, int calibrate_pin) {
00013 compass_pwm_pin = pwm_pin;
00014 compass_calibrate_pin = calibrate_pin;
00015 return 0;
00016 }
00017
00018
00019
00020
00021
00022
00023 int getCompassHeading() {
00024 pwm_delay_timer = TIMER;
00025
00026 if(!IN(compass_pwm_pin)) {
00027 compass_heading = PULSIN(compass_pwm_pin, 1);
00028 }
00029 return compass_heading - 1000;
00030 }
00031
00032 void calibrateCompass() {
00033 puts("\r\ninitiating calibration sequence\r\n\r\n");
00034 const int instructionCount = 4;
00035 char* instructions[instructionCount];
00036 char ch = 0;
00037 instructions[0] = "point robot north, then press 'c'\r\n";
00038 instructions[1] = "point robot east, then press 'c'\r\n";
00039 instructions[2] = "point robot south, then press 'c'\r\n";
00040 instructions[3] = "point robot west, then press 'c'\r\n";
00041 OUTPUT(compass_calibrate_pin);
00042 HIGH(compass_calibrate_pin);
00043 int i = 0;
00044 for(i = 0; i < 4; i++) {
00045 puts(instructions[i]);
00046 while(ch != 99) ch = getc();
00047 LOW(compass_calibrate_pin);
00048 WAIT(500);
00049 HIGH(compass_calibrate_pin);
00050 ch = 0;
00051 }
00052 INPUT(compass_calibrate_pin);
00053 puts("The compass has been calibrated.\r\n");
00054 }