diff options
Diffstat (limited to 'Venus_Skeleton/Venus_Skeleton.ino')
| -rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 55 |
1 files changed, 32 insertions, 23 deletions
diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index f766a75..82ebc3d 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -16,14 +16,6 @@ // 2 Error: Sensor Obstacle detection on the turret // 3 Error: Sensor IR Line detection -#define DEBUG true - -#ifdef DEBUG -#define DEBUG_PRINT(str) Serial.print(str) -#else -#define DEBUG_PRINT(str) do { } while (0) -#endif - #include <Servo.h> #include <HMC5883L.h> #include <Wire.h> @@ -43,6 +35,24 @@ #include "calibration_wall-e.h" //#include "calibration.eve.h" +#include "comm.h" + +// set 0 to disable debugging +// set 1 for debugging with Serial.print (default) +// set 2 for debugging over RPi serial +#ifndef DEBUG +# define DEBUG 1 +#endif + +#if DEBUG == 1 +# define DEBUG_PRINT(str) Serial.print(str) +#elif DEBUG == 2 +# define DEBUG_PRINT(str) serial_print_debug(str) +#else +# define DEBUG_PRINT(str) do { } while (0) +#endif + + // ***************** // ** DEFINITIONS ** @@ -93,10 +103,10 @@ int currValDirection = 0; int currValDirectionDegree = 0; // Timers -long timerMovementStart = 0; -long timerMovementStop = 0; -long timerInitialSequence = 0; -long timerTurret = 0; +unsigned long timerMovementStart = 0; +unsigned long timerMovementStop = 0; +//unsigned long timerInitialSequence = 0; // TODO unused? +unsigned long timerTurret = 0; // Counters int counterInit = 0; @@ -264,20 +274,17 @@ float sensMagnetometer() { return(north); // Returns angle difference } void sensCompass() { - int degree = -1; - float reading[NUM_COMPASS_CHECKS]; + int degree; float readingtotal = 0; + // take multiple samples and use the average for(int i = 0; i < NUM_COMPASS_CHECKS; i++){ readingtotal += sensMagnetometer(); } - readingtotal /= NUM_COMPASS_CHECKS; - - if (readingtotal >= 0 && readingtotal <= 360) - degree = (int) readingtotal; - - if (data.compass != degree) { + // if compass reading is sane, save it. + degree = readingtotal / NUM_COMPASS_CHECKS; + if (degree >= 0 && degree <= 360 && data.compass != degree) { data.compass = degree; // mark as changed dataToPiChangedBits |= CHANGED_SENS_COMPASS; @@ -404,6 +411,7 @@ int sensIRRemote() { } return LabFound; } + void sensBeaconTurret() { // BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT // (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners) @@ -456,9 +464,9 @@ void stopAllServos() { timerMovementStop = 0; } -bool moveTurnTo(int direction) { +void moveTurnTo(int direction) { if (direction == 0 || direction == NUM_DIRECTIONS) { - return true; + return; } // Make sure turret doesn't move @@ -777,7 +785,8 @@ void checkSample() { } } -bool confirmSample(int turretVal) { +void confirmSample(int turretVal) { + // TODO use turretVal if (!counterSampleConfirm) { stopAllServos(); // Turret direction in degrees calculated from forward. (negative is left, positive is right) |