From fba17464f5cd5c8f9fb8eb8c9c9bd5947489df69 Mon Sep 17 00:00:00 2001 From: Peter Wu Date: Wed, 10 Jun 2015 18:41:09 +0200 Subject: Integrate serial printing --- Venus_Skeleton/Venus_Skeleton.ino | 26 ++++++++++++++++++-------- Venus_Skeleton/comm.h | 14 ++++++++++++++ 2 files changed, 32 insertions(+), 8 deletions(-) diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index 5873726..f15848d 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 #include #include @@ -41,6 +33,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 ** diff --git a/Venus_Skeleton/comm.h b/Venus_Skeleton/comm.h index 7f3b43c..26e8d7b 100644 --- a/Venus_Skeleton/comm.h +++ b/Venus_Skeleton/comm.h @@ -26,3 +26,17 @@ extern serial_state_t serial_state; void handle_serial(data_t *data, int changedBits); void serial_print_debug(const char *str); + +/* simple wrappers for Serial.print compatibility */ +static inline void serial_print_debug(String str) { + serial_print_debug(str.c_str()); +} +static inline void serial_print_debug(int n) { + serial_print_debug(String(n)); +} +static inline void serial_print_debug(long n) { + serial_print_debug(String(n)); +} +static inline void serial_print_debug(float n) { + serial_print_debug(String(n)); +} -- cgit v1.2.1 From 56990d37ecc3221d56cb5417f2c50562cfed3e44 Mon Sep 17 00:00:00 2001 From: Peter Wu Date: Wed, 10 Jun 2015 18:52:48 +0200 Subject: Fix compile errors+warnings, fix compass edge case Fix signed vs. unsigned millis() comparison, remove unused vars, change functions where the return value is not checked to void. Fix edge case in senseCompass, if the reading somehow fails, then it should not change the compass value (right?). --- Venus_Skeleton/Venus_Skeleton.ino | 32 +++++++++++++++----------------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index f15848d..6bb8a4a 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -96,10 +96,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; @@ -267,20 +267,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; @@ -387,7 +384,7 @@ void sensSampleGripper() { } // sensor IR, beacon detection and recognition -int sensBeaconTurret() { +void sensBeaconTurret() { // BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT // (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners) @@ -439,9 +436,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 @@ -760,7 +757,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) @@ -796,7 +794,7 @@ bool confirmSample(int turretVal) { if (!counterMovement && counterSampleConfirm == 1) { int sign = currValDirectionDegree < 0 ? 1 : -1; currValDirectionDegree = 60 * sign; - counterSampleConfirm = 3 + counterSampleConfirm = 3; } } } -- cgit v1.2.1 From dc2d8b106942684132a39adc6e207bbc9419f893 Mon Sep 17 00:00:00 2001 From: Peter Wu Date: Wed, 10 Jun 2015 22:05:46 +0200 Subject: Rename comm.cpp -> comm.ino Due to Serial usage which needs preprocessing. --- Venus_Skeleton/comm.cpp | 110 ------------------------------------------------ Venus_Skeleton/comm.ino | 110 ++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 110 insertions(+), 110 deletions(-) delete mode 100644 Venus_Skeleton/comm.cpp create mode 100644 Venus_Skeleton/comm.ino diff --git a/Venus_Skeleton/comm.cpp b/Venus_Skeleton/comm.cpp deleted file mode 100644 index d60138e..0000000 --- a/Venus_Skeleton/comm.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/** - * Serial communication module. - */ - -#include -#include "definitions.h" -#include "dataTypes.h" -#include "comm.h" - -serial_state_t serial_state = SERIAL_UNKNOWN; - -static void handle_recv(int r); - -static void handle_ctrl(int r) { - // detect reset procedure - switch (r) { - case RESET1: - // reset request received, ack and wait for RESET2 - Serial.write(ACK1); - serial_state = SERIAL_INIT; - return; - case RESET2: - if (serial_state == SERIAL_INIT) { - // Handshake completed! - Serial.write(ACK2); - serial_state = SERIAL_READY; - } else { - // invalid state, reject it - Serial.write(OUTOFSYNC); - serial_state = SERIAL_UNKNOWN; - } - return; - case PONG: - // TODO mark as alive if PING was previously sent - return; - } - // TODO commands for navigation? Use DATA_LEN, etc? -} - -void handle_serial(data_t *data, int changedBits) { - // first attempt sync - while (Serial.available()) { - int r = Serial.read(); - - if ((r & 0xC0) == 0x80) { - // handle control packets - handle_ctrl(r); - // TODO if DATA_ESCAPE or DATA_LEN1, react appropriately - } else if (serial_state == SERIAL_READY) { - // handle normal data only when the handshake has completed. - handle_recv(r); - } - } - - if (serial_state != SERIAL_READY) { - // Hardware is not ready, do not send data. - return; - } - - // then send queued data -} - -// writes data of length len -static void serial_write_len(unsigned len) { - len--; // length on wire is one smaller (00 means 1 byte, 01 means 2, etc.) - if (len < 56) { - Serial.write(0x80 + len + 7); - } else if (len < DATA_MAX_SIZE) { - Serial.write(0x88 | (len >> 5)); - Serial.write(0xA0 | (len & 0x1F)); - } else { - // zero length or too long. - } -} - -// write remainder of data -static void serial_write_data(const char *data, unsigned len) { - for (unsigned i = 0; i < len; i++) { - char c = data[i]; - if (c & 0x80) { - Serial.write(DATA_ESCAPE); - Serial.write(c & ~0x80); - } else { - Serial.write(c); - } - } -} - -void serial_print_debug(const char *str) { - unsigned len = strlen(str); - const char c = DATA_DEBUG_PREFIX; - - // only write data when the serial connection is ready - if (serial_state != SERIAL_READY) { - return; - } - - while (len > 0) { - // len is no more than DATA_MAX_SIZE - 1 because of debug prefix - unsigned datalen = len >= DATA_MAX_SIZE ? DATA_MAX_SIZE - 1 : len; - // add one for the debug prefix - serial_write_len(1 + datalen); - serial_write_data(&c, 1); /* write prefix */ - serial_write_data(str, datalen); /* write actual data payload */ - len -= datalen; - } -} - -static void handle_recv(int r) { -} diff --git a/Venus_Skeleton/comm.ino b/Venus_Skeleton/comm.ino new file mode 100644 index 0000000..d60138e --- /dev/null +++ b/Venus_Skeleton/comm.ino @@ -0,0 +1,110 @@ +/** + * Serial communication module. + */ + +#include +#include "definitions.h" +#include "dataTypes.h" +#include "comm.h" + +serial_state_t serial_state = SERIAL_UNKNOWN; + +static void handle_recv(int r); + +static void handle_ctrl(int r) { + // detect reset procedure + switch (r) { + case RESET1: + // reset request received, ack and wait for RESET2 + Serial.write(ACK1); + serial_state = SERIAL_INIT; + return; + case RESET2: + if (serial_state == SERIAL_INIT) { + // Handshake completed! + Serial.write(ACK2); + serial_state = SERIAL_READY; + } else { + // invalid state, reject it + Serial.write(OUTOFSYNC); + serial_state = SERIAL_UNKNOWN; + } + return; + case PONG: + // TODO mark as alive if PING was previously sent + return; + } + // TODO commands for navigation? Use DATA_LEN, etc? +} + +void handle_serial(data_t *data, int changedBits) { + // first attempt sync + while (Serial.available()) { + int r = Serial.read(); + + if ((r & 0xC0) == 0x80) { + // handle control packets + handle_ctrl(r); + // TODO if DATA_ESCAPE or DATA_LEN1, react appropriately + } else if (serial_state == SERIAL_READY) { + // handle normal data only when the handshake has completed. + handle_recv(r); + } + } + + if (serial_state != SERIAL_READY) { + // Hardware is not ready, do not send data. + return; + } + + // then send queued data +} + +// writes data of length len +static void serial_write_len(unsigned len) { + len--; // length on wire is one smaller (00 means 1 byte, 01 means 2, etc.) + if (len < 56) { + Serial.write(0x80 + len + 7); + } else if (len < DATA_MAX_SIZE) { + Serial.write(0x88 | (len >> 5)); + Serial.write(0xA0 | (len & 0x1F)); + } else { + // zero length or too long. + } +} + +// write remainder of data +static void serial_write_data(const char *data, unsigned len) { + for (unsigned i = 0; i < len; i++) { + char c = data[i]; + if (c & 0x80) { + Serial.write(DATA_ESCAPE); + Serial.write(c & ~0x80); + } else { + Serial.write(c); + } + } +} + +void serial_print_debug(const char *str) { + unsigned len = strlen(str); + const char c = DATA_DEBUG_PREFIX; + + // only write data when the serial connection is ready + if (serial_state != SERIAL_READY) { + return; + } + + while (len > 0) { + // len is no more than DATA_MAX_SIZE - 1 because of debug prefix + unsigned datalen = len >= DATA_MAX_SIZE ? DATA_MAX_SIZE - 1 : len; + // add one for the debug prefix + serial_write_len(1 + datalen); + serial_write_data(&c, 1); /* write prefix */ + serial_write_data(str, datalen); /* write actual data payload */ + len -= datalen; + } +} + +static void handle_recv(int r) { +} -- cgit v1.2.1 From 5f926a60d2c80ed00665d6a09690b90ce531d8a2 Mon Sep 17 00:00:00 2001 From: Peter Wu Date: Wed, 10 Jun 2015 22:17:49 +0200 Subject: Add include guards, fix compilation with arduino-makefile Include guards are needed because files are concatenated. https://github.com/Lekensteyn/arduino-makefile (forked from https://github.com/tomswartz07/arduino-makefile) --- .gitignore | 1 + Venus_Skeleton/Makefile | 43 +++++++++++-------------------------- Venus_Skeleton/calibration_wall-e.h | 6 +++++- Venus_Skeleton/comm.h | 5 +++++ Venus_Skeleton/dataTypes.h | 5 +++++ 5 files changed, 28 insertions(+), 32 deletions(-) diff --git a/.gitignore b/.gitignore index 4fe7862..492658d 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ .*.sw? __pycache__/ *.py[oc] +bin/ diff --git a/Venus_Skeleton/Makefile b/Venus_Skeleton/Makefile index c2c5198..f150b58 100644 --- a/Venus_Skeleton/Makefile +++ b/Venus_Skeleton/Makefile @@ -1,35 +1,16 @@ -#CC = /usr/share/arduino/hardware/tools/avr/bin/avr-gcc -CC = avr-gcc -WFLAGS = -Wall -Wextra -Wno-attributes -WFLAGS += -fdiagnostics-color=auto -EXTRA_CFLAGS = $(shell cat .syntastic_c_config) -O2 $(CFLAGS) -MY_SOURCES = Venus_Skeleton.ino comm.cpp -MY_OBJECTS = $(patsubst %.cpp,%.o,$(patsubst %.ino,%.o,$(MY_SOURCES))) -LIBS_SOURCES = $(wildcard libs/*/*.cpp) -LIBS_OBJECTS = $(patsubst %.cpp,%.o,$(LIBS_SOURCES)) -SOURCES = $(MY_SOURCES) $(LIBS_SOURCES) -OBJECTS = $(MY_OBJECTS) $(LIBS_OBJECTS) -PROGRAM ?= main +PROJECT = Venus_Skeleton +ARDUINO_MODEL = uno +#PORT = /dev/ttyACM* +ARDUINO = 163 +#ARDUINO_DIR = /usr/share/arduino +#ARDUINO_VARIANT = $(ARDUINO_DIR)/hardware/arduino/avr/variants/standard -_V_0 = @ -_V = $(_V_$(V)) +ARDUINO_LIBS = Serial Servo Wire +USER_LIBDIR = ./libs +USER_LIBS = HMC5883L IRremote TrueRandom -.PHONY: main clean check -check: - rm -f $(MY_OBJECTS) - $(MAKE) $(MY_OBJECTS) -# Note: does not compile due to link errors. -$(PROGRAM): $(OBJECTS) - $(_V)$(CC) $(WFLAGS) $(EXTRA_CFLAGS) $(OBJECTS) -o $@ +CEXTRA = -fdiagnostics-color=auto +#CEXTRA += -Wall -Wextra -Wno-attributes -clean: - $(_V)rm -f $(PROGRAM) $(OBJECTS) - -# Not my code... -libs/%.o: libs/%.cpp - $(_V)$(CC) -w $(EXTRA_CFLAGS) -c -o $@ $< -%.o: %.cpp - $(_V)$(CC) $(WFLAGS) $(EXTRA_CFLAGS) -c -o $@ $< -%.o: %.ino - $(_V)$(CC) $(WFLAGS) $(EXTRA_CFLAGS) -c -x c++ -o $@ $< +include ~/Arduino/arduino-makefile/Makefile diff --git a/Venus_Skeleton/calibration_wall-e.h b/Venus_Skeleton/calibration_wall-e.h index 16e0ebe..c859a84 100644 --- a/Venus_Skeleton/calibration_wall-e.h +++ b/Venus_Skeleton/calibration_wall-e.h @@ -1,3 +1,5 @@ +#ifndef CALIBRATION_WALL_E_H +#define CALIBRATION_WALL_E_H // WALLY configuration file #define calObstacleTurretMaxDist 50 @@ -27,4 +29,6 @@ #define CAL_TURRET_TO_CENTER_DIST 6 -#define CAL_SAMPLE_TURRET_MAX_DIST 27 \ No newline at end of file +#define CAL_SAMPLE_TURRET_MAX_DIST 27 + +#endif /* !CALIBRATION_WALL_E_H */ diff --git a/Venus_Skeleton/comm.h b/Venus_Skeleton/comm.h index 26e8d7b..bca5aab 100644 --- a/Venus_Skeleton/comm.h +++ b/Venus_Skeleton/comm.h @@ -1,3 +1,6 @@ +#ifndef COMM_H +#define COMM_H + typedef enum { RESET1 = 0x81, RESET2 = 0x82, @@ -40,3 +43,5 @@ static inline void serial_print_debug(long n) { static inline void serial_print_debug(float n) { serial_print_debug(String(n)); } + +#endif /* !COMM_H */ diff --git a/Venus_Skeleton/dataTypes.h b/Venus_Skeleton/dataTypes.h index f19eaf5..2f2aa69 100644 --- a/Venus_Skeleton/dataTypes.h +++ b/Venus_Skeleton/dataTypes.h @@ -1,3 +1,6 @@ +#ifndef DATATYPES_H +#define DATATYPES_H + // **************** // ** Data types ** // **************** @@ -63,3 +66,5 @@ typedef enum { OPMODE_LABSEQUENCE, OPMODE_ERROR } opmode_t; + +#endif /* !DATATYPES_H */ -- cgit v1.2.1