//****************************************************************************
//
// Description of Software:
//
// Mitsubishi 3000gt/Stealth Controller Program
// for controlling stock OEM mitsubishi ECS Struts.
// Mitsubish 3000gt model years 1991 - 1999.
// Note:models years 1995-1999 are retrofit of this system+Original Struts.
// Hardware Platform: ATAMega 328 MicroController
//
// Copyright (C) 2010,2011,2012,2013,2014,2015,2016
// Marcus Diaz, RenegadeTechWorks, LLC
//
//****************************************************************************
// Licensing:
// Licensed under GNU GPL Version 2
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation under Version 2 of the License
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//****************************************************************************
// VERSION HISTORY
//
//--------------------------------------------------------------------
// SW Ver : v3.00
// HWDesign: ECM M03 PCB Rev
// Date: 03/08/2016
// Comments:
// This code represents a branch and re-write for the Model 03 of
// of theTechWorks ECS controller.
// The new Gen3/ Model03(M03) Controller uses a modified MO1 design
// New Features are
// -- freed up I/O ports to support I2C buss devices
// -- includes add on daughther board support for an DLH303 6-axis Accellerometer
// -- Code has been rewritten in more Ojbect Oriented style.
// -- Auto Mode added to use accelerometer data to auto set struts
// -- Manual Mode remains
//
//
//--------------------------------------------------------------------
// SW Ver : v3.01
// HWDesign: ECM M03 PCB Rev
// Date: 07/05/2016
// Comments:
// - Added to startup sequence to retrieve from eprom the
// default startup mode
// - Added Sport/Tour light blinking patterns to startup sequence that
// shows what default startup mode is
// - Added Set Led Brightness Mode
// - Added Save current mode as default startup mode
// - Modes are:
// 1 push - set next stiffnes setting
// 2 pushes - toggle auto/manual modes
// 3 pushes - set led brightness
// 4 pushes - save curreent mode as default startup mode
// 5 pushes - read diagnostics mode
// 6 pushes - run diagnostics mode
//--------------------------------------------------------------------
// SW Ver : v3.01
// HWDesign: ECM M03 PCB Rev 3.1
// Date: 07/09/2016
// Comments:
// - fixed logic in sportTourLoopLights routine to correctly display
// command mode & signal wire failures
// - changed manual strut mode to do struts one at a time
//
///--------------------------------------------------------------------
// SW Ver : v3.01
// HWDesign: ECM M03 PCB Rev 3.1
// Date: 07/16/2016
// Comments:
// - changed manual strut mode back to parrallel mode
// - change H/W design to use 1.5 amp 9v VReg for old strut
///--------------------------------------------------------------------
// SW Ver : v3.2
// HWDesign: ECM M03 PCB Rev 3.1
// Date: 07/25/2016
// Comments:
// - added modes to set threshold values
// - added modes to Calibrate accelerometer
// - added code to use accelerometer calibration values
// - added code in setStrutMode to turn motors back on for old struts
///--------------------------------------------------------------------
// SW Ver : v3.3 Official Production Release Used in Shipping M03 Controllers
// HWDesign: ECM M03 PCB Rev 3.1
// Date: 10/17/2016
// Comments:
// - added logging Mode
// - added firmware version & startup values to print at startup
// - added seperate maxtries params for Auto vs Manual strut modes
// - added LED = OFF when strut state cannot be determined during transition
// - added LED = GREENYELLOW if strut goes into mode but cannot stay there
// - reworked the logic & assignments in readStrutState() - to match reality
// - changed sport tour lights for automode to show max target across the four struts
///***********************************************************************************
#include <EEPROM.h>
#include <FastLED.h>
#include <PinChangeInt.h>
#include <Wire.h>
//
//********* Firmware Version ****************
//
#define FIRMWARE_VERSION "Firmware Version M03 v3.3"
//
//********* LSM303DLHC Accelerometer/ Compass I2C Addresses ****************
//
#define LSM303_ADDRESS_ACC (0x32 >> 1) // 0011001x
#define LSM303_ADDRESS_MAG (0x3C >> 1) // 0011110x
#define MANUAL 1
#define AUTO 2
#define DIAG 3
#define READDIAG 4
#define SETSTARTUP 5
#define SETLED 6
#define SETTHRESHOLD 7
#define CALIBRATEACCEL 8
#define NOMINAL 0
#define CHANGING 1
#define FAILED 2
#define OLDSTRUTS 3
#define PRENOMINAL 4
#define STRUT_FL 0
#define STRUT_FR 1
#define STRUT_RL 2
#define STRUT_RR 3
#define HARD 1
#define MEDIUM 2
#define SOFT 3
#define UNKNOWN 7
#define NOCHANGE 10
#define STARTUP 11
#define D_RED 1
#define D_GREEN 2
#define D_BLUE 3
#define D_TURQUOIS 4
#define D_PURPLE 5
#define D_YELLOW 6
#define D_GREENYELLOW 7
#define D_STARTUP 8
#define D_HARD 9
#define D_MEDIUM 10
#define D_SOFT 11
#define D_WHITE 12
#define D_OFF 13
#define D_UNKNOWN 14
#define RED100 0xFF0000 // Bright Red
#define GREEN100 0x00FF00 // Bright Green
#define BLUE100 0x0000FF // Bright Blue
#define RED001 0x010000 // Faint red
#define RED050 0x800000 // 1/2 red (0x80 = 128 out of 256)
#define RED025 0x300000 // 25% RED
#define GREEN050 0x008000 // half Green
#define GREEN025 0x003000 // 25% GREEN
#define BLUE050 0x000080 // half Blue
#define BLUE025 0x000030 // 25% BLUE
#define WHITE015 0x151515 // 15% White
#define WHITE010 0x101010 // 10% White
#define WHITE005 0x050505 // 05% White
#define WHITE001 0x020202 // 05% White
#define WHITE100 0xFFFFFF // 100% White
#define LEDOFF 0x000000
#define RED 0xFF0000
#define ORANGE 0xFF5500
#define ORANGEYELLOW 0xFFA000
#define YELLOW 0xFFFF00
#define YELLOWGREEN 0xA0FF00
#define GREENYELLOW 0x50FF00
#define GREEN 0x00FF00
#define GREENCYAN 0x00FF4B
#define CYAN 0x00FFFF
#define CYANBLUE 0x00A0FF
#define BLUECYAN 0x005AFF
#define BLUE 0x0000FF
#define BLUEVIOLET 0x4800FF
#define VIOLETBLUE 0x7500FF
#define MAGENTA 0xFF00FF
#define PINKDEEP 0xFF1493
#define PINKHOT 0xFF69B4
#define PINK 0xF3967A
#define VIOLET 0xEE82EE
#define ERROR 1
#define NOERROR 0
#define CLEAR 1
#define DONT_CLEAR 0
#define SIGNAL_WIRE_FAILURE 2
#define MOTOR_COMMAND_FAILURE 1
#define NO_FAILURES 0
#define AUTO_MAXTRIES 475 // maxtries = 475 = approximately 2.75 secs of Motor on
// AUTO_MAXTRIES time equivalent should be less than STRUT_HOLD_INTERVAL
#define MANUAL_MAXTRIES 10000 // maxtries = 10000 = approximately 4 secs of Motor on
#define MAXFAILURES 100 // maxiumum number of faliures allowed for a strut during a power on cycle
#define MAXLIGHTLOOPS 20
#define ROTARY 1
#define PUSHBUTTON 2
#define SAMEMODE 1
#define NEXTMODE 2
#define LED_LEVEL1 1 // minimum brightness level
#define LED_LEVEL2 2
#define LED_LEVEL3 3
#define LED_LEVEL4 4 // Maximum brightness level
// ******************************************************************
//
// EPROM ADDRESSES
//
// ******************************************************************
#define EPROM_LEDMODE 1 // address in EPROM for default start up LED Brightness Level
#define EPROM_CONTROLLER_MODE 3 // address in EPROM for default start up Controller Mode (Auto or Manual)
#define EPROM_STRUT_MODE 5 // address in EPROM for default start up Strut Stiffness Mode
#define EPROM_MEDIUM_THRESHOLD 7 // address in EPROM for default start up Strut Stiffness Mode
#define EPROM_HARD_THRESHOLD 9 // address in EPROM for default start up Strut Stiffness Mode
#define EPROM_ACC_X_NEG_1G 11 // address in EPROM for Accelerometer Negative X axis 1G Calibration Value
#define EPROM_ACC_Y_NEG_1G 13 // address in EPROM for Accelerometer Negative Y axis 1G Calibration Value
#define EPROM_ACC_Z_NEG_1G 15 // address in EPROM for Accelerometer Negative Z axis 1G Calibration Value
#define EPROM_ACC_X_POS_1G 17 // address in EPROM for Accelerometer Positive X axis 1G Calibration Value
#define EPROM_ACC_Y_POS_1G 19 // address in EPROM for Accelerometer Positive Y axis 1G Calibration Value
#define EPROM_ACC_Z_POS_1G 21 // address in EPROM for Accelerometer Positive Z axis 1G Calibration Value
///////////////////////////////////////////////////////////////////////////////////////
//
// Map Board I/O Pins to World
//
// /////////////////////////////////////////////////////////////////////////////////////
// Front Left Strut Pins
#define STRUT_FL_SWITCH1 3
#define STRUT_FL_SWITCH2 2
#define STRUT_FL_MOTOR 10
// Front Right Strut Pins
#define STRUT_FR_SWITCH1 5
#define STRUT_FR_SWITCH2 4
#define STRUT_FR_MOTOR 11
// Rear Left Strut Pins
#define STRUT_RL_SWITCH1 7
#define STRUT_RL_SWITCH2 6
#define STRUT_RL_MOTOR 14
// Rear Right Strut Pins
#define STRUT_RR_SWITCH1 9
#define STRUT_RR_SWITCH2 8
#define STRUT_RR_MOTOR 15
//
// LED Light Ports
//
#define DATA_PIN 16 // Data pin that led data will be written out over
//
// Switch State
//
#define NO_PUSH_TO_PROCESS 0
#define NEW_PUSH_OCCURED 1
#define CONTINUE_PROCESSING 2
#define ACC_DATA_POINTS 10 // maximum number of Acceleration DataPoints
#define STRUT_HOLD_INTERVAL 3000 // numbers of milliseconds to hold a strut higher state;
#define X_AXIS 0 // Acceleromter X Axis in Array
#define Y_AXIS 1 // Acceleromter Y Axis in Array
#define Z_AXIS 2 // Acceleromter Z Axis in Array
//
// Logging modes
//
#define LOG_TIME 1
#define LOG_STRUTS_NAME 2
#define LOG_ACCELEROMETER 3
//***************************************************************************************
//
// Create and Intialize (some ) Global Variables
//
//***************************************************************************************
//
// Map Physical Accelerometer axis & sign to normalized variables
//
int ForwardAxis = Y_AXIS; // default to Y axis
int ForwardSign = 1; // default to Y+ +Acc = Acceleration
int LeftRightAxis = Z_AXIS; // default to Z axis
int LeftRightSign = 1; // default to Z+ +Acc = Left Turn
int UpDownAxis = X_AXIS; // default to X axis
int UpDownSign = 1; // default to X+ +Acc = Up
int accelDataPoints = 10; // default number of acceleration data points to average
float ForwardAccelerationArray[ACC_DATA_POINTS] ;
float LeftRightAccelerationArray[ACC_DATA_POINTS] ;
float UpDownAccelerationArray[ACC_DATA_POINTS] ;
float ForwardAcceleration;
float LeftRightAcceleration;
float UpDownAcceleration;
//
// X Y Z Axis Calibration Variables
//
int XAxisNegative1gCalibration = 0;
int YAxisNegative1gCalibration = 0;
int ZAxisNegative1gCalibration = 0;
int XAxisPositive1gCalibration = 0;
int YAxisPositive1gCalibration = 0;
int ZAxisPositive1gCalibration = 0;
//
// variables to contain integer level value that corresponds to actual decimal value of threshold
// 1 = 0.1g 2= 0.2g 3= 0.3g 4= 0.4g 5 =0.5g 6= 0.6g 7=0.7g
int mediumThresholdLevel;
int hardThresholdLevel;
//
// Default Threshold variables for Auto Mode Strut transitions in G's of force
//
float mediumThreshold = 0.2;
float hardThreshold = 0.4;
//Mode Selector Switch Pin
#define SELECTOR A3 // A3=Analog pin A3=physical pin 26 on AT328) is part of the PinChangeInt.h library defines
int selectorSwitchPin = 17;
//Switch Type Selector Option Pin
int switchTypeSelect = 1;
//LED Type Selector Option Pin
int LEDTypeSelect = 0;
// Sport Light Port
int SportLight = 12;
// Tour Light Port
int TourLight = 13;
//Print Mode 1=log/print 0=no logging
int logMode = 0;
int logSequence = 0;
long ledHardValue ;
long ledMediumValue;
long ledSoftValue;
long ledRedValue;
#define FL_LED 0 // Front Left Strut LED
#define FR_LED 1 // Front Right Strut LED
#define RL_LED 2 // Rear Left Strut LED
#define RR_LED 3 // Rear Right Strut LED
int lightLoopCounter = MAXLIGHTLOOPS;
int lightLoopCounterValue;
int accelerometerStrutTargets[4];
int previousMode;
//
// Allocate Structure For LED Strip
//
#define NUM_LEDS 8
struct CRGB ledColors[NUM_LEDS];
long ledCurrentValue[4];
unsigned long lightCycleStartTime =0;
unsigned long signalWireFailureCycleStartTime;
unsigned long commandFailureCycleStartTime;
// **********************************
// Selector Switch variables & Intterupt function
// **********************************
/*
// Notice that anything that gets modified inside an interrupt, that I wish to access
// outside the interrupt, is marked "volatile". That tells the compiler not to optimize
// them.
*/
volatile uint8_t pushCount=0;
volatile uint8_t pushEvent= NO_PUSH_TO_PROCESS;
volatile unsigned long pushTime=0;
int firstTime=1;
//
// Interupt functgion for Selector Switch
//
void selectorInterruptfunc() {
pushCount = pushCount +1;
pushTime=millis();
pushEvent = NEW_PUSH_OCCURED;
}
// **********************************
// Strut Object Class Definition
// **********************************
class Strut{
public:
Strut(){
}
~Strut(){}
//************************************
int strutPosition() {
return vstrutPosition;
}
int setStrutPosition(int position) {
vstrutPosition = position;
}
//************************************
int desiredState() {
return vdesiredState;
}
int setDesiredState(int target) {
vdesiredState = target;
}
//************************************
unsigned long lastChangeTime() {
return vlastChangeTime;
}
void setLastChangeTime() {
vlastChangeTime= millis();
}
//************************************
int actualState(int display) {
vstate = readStrutState(vstrutPosition,display);
return vstate;
}
//***********************************
int transitionStatus() {
return vtransitionStatus;
}
int setTransitionStatus(int status) {
vtransitionStatus = status;
}
//************************************
int switch1() {
return vs1Pin;
}
int setSwitch1(int pin) {
vs1Pin = pin;
}
//************************************
int switch2() {
return vs2Pin;
}
int setSwitch2(int pin) {
vs2Pin = pin;
}
//************************************
int motorPin() {
return vmotorPin;
}
int setMotor(int pin) {
vmotorPin = pin;
}
int motorOn() {
digitalWrite(vmotorPin, HIGH);
}
int motorOff() {
digitalWrite(vmotorPin, LOW);
}
//************************************
int tryCount() {
return vtryCount;
}
void clearTryCount() {
vtryCount =0;
}
void incrementTryCount() {
vtryCount++;
}
//************************************
int failures() {
return vtotalFailures;
}
void clearFailures() {
vtotalFailures = 0;
}
void incrementFailures() {
vtotalFailures++;
}
//************************************
int errorStatus() {
return verrorStatus;
}
void setErrorStatus(int status) {
verrorStatus = status;
}
private:
int vstrutPosition; // Strut Postion: STRUT_FL _FR _RL _RR
int vstate; // Actual State of the Struts: HARD, MEDIUM, SOFT
int vdesiredState; // Desired Target State based on last command loop execution: H, M, S
int vtransitionStatus; // Indicate if the Strut in transition from last command mode to a new desired target or
// Or is stable and in quiet state
// Modes Are:
// NOMINAL = Strut Actual Mode matches the DesiredCommand Target.
// Motors should be off
// CHANGING = Strut In a commanded transitional state changing
// from Medium to Hard for example. Motors Should be On
// Neutral = Everything has been shut down and Motors are off. Command Target is not guaranteed
// Strut is whatever state it is in.
int verrorStatus = NO_FAILURES; // Indicates current Error Status of Strut Possible Error Status modes are
// NO_FAILURES Means no failure has previously Been detected Since Last Command given
// SIGNAL_WIRE_FAILURE Means a signal wire failure has been previously detected
// MOTOR_COMMAND_FAILURE Means a Motor Command failure has been previously detected
int vtryCount = 0; // Indicates how many times previously a Strut has has passed thru the command loop
// while trying to achive the desired targetState
int vtotalFailures = 0; // Indicates how many total times previously a Strut has exceeded maxtries/tryCount
// while trying to achive the desired targetState
int vs1Pin;
int vs2Pin;
int vmotorPin;
unsigned long vlastChangeTime;
};
// *********************************************************
// Controller Object Class Definition
// *********************************************************
class Controller{
public:
Controller(){
}
~Controller(){}
int readSwitch(){
return(readControllerSwitch());
}
int mode() {
return vcontrollerMode;
}
int setMode(int mode) {
vcontrollerMode = mode;
}
int strutMode() {
return vcontrollerStrutMode;
}
int setControllerStrutMode(int mode) {
vcontrollerStrutMode = mode;
}
int selectorType() {
return(vselectorType);
}
int setSelectorType(int type) {
vselectorType = type;
}
//
// Maybe put read switch as function of Controller??
//
private:
int vcontrollerMode; // Auto, Manual, Diag
int vcontrollerStrutMode; // Hard, Med, Soft
int vselectorType = ROTARY; // selectorType captures what kind of selector switch is hooked up to unit
};
//
// ** instantiate & Initialize Strut Array Object **
//
Strut myStruts[4];
//
// ** instantiate & Initialize Controller Array Object **
//
Controller myController;
int ledMode = LED_LEVEL2;
// Previous State of Selector Switch
int currentSelectorState = STARTUP;
///////////////////////////////////////////////////////////////////////////////////////
// setup initializes startup
///////////////////////////////////////////////////////////////////////////////////////
void setup(){
int strutStateFL;
int strutStateFR;
int strutStateRL;
int strutStateRR;
int selectorValue;
int diagLoopNumber;
int epromValue;
// Initialize the Sport & Tour Light OutPut Pins
pinMode(SportLight, OUTPUT);
pinMode(TourLight, OUTPUT);
//
// intialize all the motor & Switch state pins
//
myStruts[0].setSwitch1(STRUT_FL_SWITCH1);
myStruts[0].setSwitch2(STRUT_FL_SWITCH2);
myStruts[0].setMotor(STRUT_FL_MOTOR);
myStruts[0].setStrutPosition(STRUT_FL);
myStruts[1].setSwitch1(STRUT_FR_SWITCH1);
myStruts[1].setSwitch2(STRUT_FR_SWITCH2);
myStruts[1].setMotor(STRUT_FR_MOTOR);
myStruts[1].setStrutPosition(STRUT_FR);
myStruts[2].setSwitch1(STRUT_RL_SWITCH1);
myStruts[2].setSwitch2(STRUT_RL_SWITCH2);
myStruts[2].setMotor(STRUT_RL_MOTOR);
myStruts[2].setStrutPosition(STRUT_RL);
myStruts[3].setSwitch1(STRUT_RR_SWITCH1);
myStruts[3].setSwitch2(STRUT_RR_SWITCH2);
myStruts[3].setMotor(STRUT_RR_MOTOR);
myStruts[3].setStrutPosition(STRUT_RR);
for (int i=0; i < 4; i++){
initializeMotorIOPin(myStruts[i].motorPin());
initializeStrutIOPin(myStruts[i].switch1());
initializeStrutIOPin(myStruts[i].switch2());
}
//
// initialize current led value array
//
for (int i=0; i < 4; i++){
ledCurrentValue[i] = 0x00;
}
//
//
// Initialize Accelerometer
//
Wire.begin(); // Start up I2C, required for LSM303 communication
initAccelerometer();
// ************************************************************
//
// LED Setup & Initialization
//
// ************************************************************
FastLED.addLeds<WS2812B, DATA_PIN, RGB>(ledColors, NUM_LEDS);
// Clear Out the LED arrays
memset(ledColors, 0, NUM_LEDS * sizeof(struct CRGB));
LEDS.show(); //Push the current color frame to the LEDs
//
// initialize current led value array
//
for (int i=0; i < 4; i++){
ledCurrentValue[i] = 0x00;
}
// analyze Selector TYPE Input Pin To figure out if Rotary of Pushbutton is connected
// HIGH = Rotary LOW = PUSHBUTTON
pinMode(switchTypeSelect, INPUT);
digitalWrite(switchTypeSelect,HIGH);
// Read the pin to see if it's HIGH or LOW
if(digitalRead(switchTypeSelect) == HIGH){
myController.setSelectorType(PUSHBUTTON);
/* original non-interrupt switch config
pinMode(selectorSwitchPin, INPUT);
*/
//
// new code to use interrupt for pushbutton switch
//
pinMode(SELECTOR, INPUT_PULLUP);
attachPinChangeInterrupt(SELECTOR, selectorInterruptfunc , RISING); // Rising state change will trigger the interrupt.
// attachPinChangeInterrupt(SELECTOR, selectorInterruptfunc , CHANGE); // Any state change will trigger the interrupt.
// attachPinChangeInterrupt(SELECTOR, selectorInterruptfunc , FALLING); // Falling state change will trigger the interrupt.
/*
digitalWrite(selectorSwitchPin, HIGH);
*/
lightLoopCounterValue = 900; // the lightloopcounter is timing dependent based on selector type
diagLoopNumber = 450;
}else{
myController.setSelectorType(ROTARY);
pinMode(selectorSwitchPin, INPUT);
digitalWrite(selectorSwitchPin, LOW);
lightLoopCounterValue = 60; // the lightloopcounter is timing dependent based on selector type
diagLoopNumber = 40;
}
lightLoopCounter = lightLoopCounterValue;
//
// Retrieve LED Brigtness level from EPROM - if not within range set it to 2nd level and write it back
//
ledMode = eepromReadInt(EPROM_LEDMODE);
//
// Make sure the value is with in range
//
if (ledMode != LED_LEVEL1 && ledMode != LED_LEVEL2 && ledMode != LED_LEVEL3 && ledMode != LED_LEVEL4){
// It's not within range - set to LEVEL two and write it back
ledMode = LED_LEVEL2;
eepromWriteInt(EPROM_LEDMODE,ledMode);
}
//
// Set the LED brightness values based on EProm value
//
setLedValues();
// EPROM_MEDIUM_THRESHOLD
// EPROM_HARD_THRESHOLD
//
// Retrieve medium threshold level from EPROM - if not within range set it to 2nd level and write it back
//
mediumThresholdLevel = eepromReadInt(EPROM_MEDIUM_THRESHOLD );
//
// Make sure the value is with in range
//
if (mediumThresholdLevel < 1 || mediumThresholdLevel > 7 ){
// It's not within range - set to LEVEL two and write it back
mediumThresholdLevel = 2 ;
eepromWriteInt(EPROM_MEDIUM_THRESHOLD, mediumThresholdLevel );
}
//
// Set g threshold level based on EEProm
//
mediumThreshold = float( mediumThresholdLevel/10.0) ;
//
// Retrieve hard threshold level from EPROM - if not within range set it to 2nd level and write it back
//
hardThresholdLevel = eepromReadInt(EPROM_HARD_THRESHOLD );
//
// Make sure the value is with in range
//
if (hardThresholdLevel < 1 || hardThresholdLevel > 9 ){
// It's not within range - set to LEVEL two and write it back
hardThresholdLevel = 4 ;
eepromWriteInt(EPROM_HARD_THRESHOLD, hardThresholdLevel );
}
//
// Set g threshold level based on EEProm
//
hardThreshold = float( hardThresholdLevel/10.0) ;
//
// Retrieve Strut Mode from EPROM
epromValue= eepromReadInt(EPROM_STRUT_MODE);
/*
Serial.print(F(" Eprom Strut Mode ="));
Serial.println(epromValue);
*/
//
// if not valid eprom value set it to Soft;
//
//
if (epromValue != HARD && epromValue != MEDIUM && epromValue != SOFT ){
// It's not within range - set to Soft as default
epromValue = SOFT;
eepromWriteInt(EPROM_STRUT_MODE,epromValue);
}
myController.setControllerStrutMode(epromValue);
//
// Retrieve X Axis Neg 1g Galibration Value From Eprom if not with in range set to default value and write back
//
XAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_X_NEG_1G);
if (XAxisNegative1gCalibration >32000 || XAxisNegative1gCalibration < 1 ){
// It's not within range - set to default and write it back
XAxisNegative1gCalibration = 16380;
eepromWriteInt(EPROM_ACC_X_NEG_1G , XAxisNegative1gCalibration);
}
//
// Retrieve Y Axis Neg 1g Galibration Value From Eprom if not with in range set to default value and write back
//
YAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Y_NEG_1G);
if (YAxisNegative1gCalibration >32000 || YAxisNegative1gCalibration < 1 ){
// It's not within range - set to default and write it back
YAxisNegative1gCalibration = 16380;
eepromWriteInt(EPROM_ACC_Y_NEG_1G , YAxisNegative1gCalibration);
}
//
// Retrieve Z Axis Neg 1g Galibration Value From Eprom if not with in range set to default value and write back
//
ZAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Z_NEG_1G);
if (ZAxisNegative1gCalibration >32000 || ZAxisNegative1gCalibration < 1 ){
// It's not within range - set to default and write it back
ZAxisNegative1gCalibration = 16380;
eepromWriteInt(EPROM_ACC_Z_NEG_1G , ZAxisNegative1gCalibration);
}
//
// Retrieve X Axis Positive 1g Galibration Value From Eprom if not with in range set to default value and write back
//
XAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_X_POS_1G);
if (XAxisPositive1gCalibration > 32000 || XAxisPositive1gCalibration < 1 ){
// It's not within range - set to default and write it back
XAxisPositive1gCalibration = 16380;
eepromWriteInt(EPROM_ACC_X_POS_1G , XAxisPositive1gCalibration );
}
//
// Retrieve Y Axis Positive 1g Galibration Value From Eprom if not with in range set to default value and write back
//
YAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Y_POS_1G);
if (YAxisPositive1gCalibration > 32000 || YAxisPositive1gCalibration < 1 ){
// It's not within range - set to default and write it back
YAxisPositive1gCalibration = 16380;
eepromWriteInt(EPROM_ACC_Y_POS_1G , YAxisPositive1gCalibration );
}
//
// Retrieve Z Axis Positive 1g Galibration Value From Eprom if not with in range set to default value and write back
//
ZAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Z_POS_1G);
if (ZAxisPositive1gCalibration > 32000 || ZAxisPositive1gCalibration < 1 ){
// It's not within range - set to default and write it back
ZAxisPositive1gCalibration = 16380;
eepromWriteInt(EPROM_ACC_Z_POS_1G , ZAxisPositive1gCalibration );
}
//
// Turn on the Sport / Tour Lights at startup for 1 sec as test to driver to show they are working.
//
setLights(SportLight,HIGH);
setLights(TourLight,HIGH);
delay(1000);
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
delay(300);
//
// Retrieve Controller Mode from EPROM
//
epromValue= eepromReadInt(EPROM_CONTROLLER_MODE);
/*
Serial.print(F("Eprom Controller Mode ="));
Serial.print(epromValue);
*/
//
// if controller mode not Auto or Manual set to manual
//
if (epromValue != AUTO && epromValue != MANUAL ){
// It's not within range - set to Manual as default
epromValue = MANUAL;
eepromWriteInt(EPROM_CONTROLLER_MODE,epromValue);
}
myController.setMode(epromValue);
//
// Based on Controller default startup mode flash lights accordingly
//
if(epromValue == AUTO){
//
// Blink both lights alternating(3 times) to indicate auto mode
//
for(int i=0;i<2;i++){
setLights(SportLight,HIGH);
setLED(STRUT_FR,D_WHITE);
setLights(TourLight,LOW);
setLED(STRUT_FL,D_OFF);
delay(250);
setLights(SportLight,LOW);
setLED(STRUT_FR,D_OFF);
setLights(TourLight,HIGH);
setLED(STRUT_FL,D_WHITE);
delay(250);
}
}else if (epromValue == MANUAL){
//
// Blink both lights at together (3 times) to indicate manual mode
//
blinkBothLights(3,250,1,0,1);
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
}
lightCycleStartTime =millis();
pushCount=0;
//
// ********* Set Up for Logging & Print some Initial Values
//
Serial.begin(115200);
while (!Serial) {
; // wait for serial port to connect. Needed for Leonardo only
};
Serial.println(F("*************************************************************************"));
Serial.println(F("Renegade Techworks Mitsubishi 3000gt/Stealth ECS Controller"));
Serial.println(FIRMWARE_VERSION);
Serial.print(F("Strut G-Force Medium Threshold: "));
Serial.println( mediumThreshold);
Serial.print(F("Strut G-Force Hard Threshold: "));
Serial.println( hardThreshold );
readPrintEpromValues();
}
/////////////////////////////////////////////////////////////////////////////////////
//
// main loop
//
/////////////////////////////////////////////////////////////////////////////////////
void loop()
{
//
// Check for updates to the state of the Controller Mode
//
//
myController.readSwitch();
if(logMode) doLogMode(LOG_TIME,0,0);
switch(myController.mode()){
case(MANUAL):
if(logMode) doLogMode(LOG_ACCELEROMETER,0,0);
setManualStrutTargets();
setStruts();
// setManualStruts();
break;
case(AUTO):
readAccelerometerValues();
setAccelerometerTargets();
setAutoStrutTargets();
setStruts();
break;
case(SETLED):
setLed();
break;
case(SETSTARTUP):
setStartup();
break;
case(DIAG):
runDiagnostics();
break;
case(READDIAG):
readDiagnostics();
break;
case(SETTHRESHOLD):
setAutoThreshold();
break;
case(CALIBRATEACCEL):
calibrateAccelerometer();
break;
}
//
// display current state of Sport-Tour lights
//
setLoopTourSportLights();
}
//*************************************************************************************
//
//
// setAutoThreshold()
//
//
//************************************************************************************
void setAutoThreshold(){
// Turn off Lights while setting LEDs
setLights(SportLight,LOW);
setLights(TourLight,LOW);
int autoThresholdSet = 0;
mediumThresholdLevel = 2;
hardThresholdLevel = 4;
/*
* Stay in this loop until button is pressed multple times to indicate save
*/
while(1){
//read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
switch(readPushButtonSwitch()){
case NOCHANGE:
break;
case 1:
//
// Ok they want to change to next Threshold Values
//
if(autoThresholdSet == 5) autoThresholdSet =1;
else autoThresholdSet++;
//
// Blink both lights at together x times to indicate which threshold set is selected
//
blinkBothLights(autoThresholdSet,500,1,0,1);
switch(autoThresholdSet){
case 1:
mediumThresholdLevel = 1;
hardThresholdLevel = 3;
break;
case 2:
mediumThresholdLevel = 2;
hardThresholdLevel = 4;
break;
case 3:
mediumThresholdLevel = 3;
hardThresholdLevel = 5;
break;
case 4:
mediumThresholdLevel = 4;
hardThresholdLevel = 6;
break;
case 5:
mediumThresholdLevel = 5;
hardThresholdLevel = 7;
break;
}
break;
case 2:
case 3:
case 4:
//
// Ok they want to save the auto Threshhold values and exit
//
eepromWriteInt(EPROM_MEDIUM_THRESHOLD,mediumThresholdLevel);
eepromWriteInt(EPROM_HARD_THRESHOLD,hardThresholdLevel);
mediumThreshold = float( mediumThresholdLevel/10.0) ;
hardThreshold = float( hardThresholdLevel/10.0) ;
//
// Blink both lights at together (7 times) to indicate saving Threshold mode
//
blinkBothLights(7,500,1,0,1);
delay(500);
//
// Restore the previous state of the controller
//
myController.setMode(previousMode);
return;
break;
}
}
}
//*************************************************************************************
//
//
// setLed()
//
//
//************************************************************************************
void setLed(){
// Turn off Lights while setting LEDs
setLights(SportLight,LOW);
setLights(TourLight,LOW);
/*
* Display LedValues
*/
setLedValues();
/*
* Stay in this loop until button is pressed multple times to indicate save
*/
while(1){
//read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
switch(readPushButtonSwitch()){
case NOCHANGE:
break;
case 1:
//
// Ok they want to change to next LED Value
//
if(ledMode == 4) ledMode =1;
else ledMode++;
setLedValues();
break;
case 2:
case 3:
case 4:
//
// Ok they want to save the current LED Brightness value and exit
//
eepromWriteInt(EPROM_LEDMODE,ledMode);
//
// Blink both lights at together (3 times) to indicate LED mode
//
blinkBothLights(3,500,1,0,1);
//
// Restore the previous state of the controller
//
myController.setMode(previousMode);
return;
break;
}
}
}
//*************************************************************************************
//
//
// setLedValues
//
//
//************************************************************************************
void setLedValues(){
switch(ledMode){
case LED_LEVEL1:
//SET the LEDs to Minimum Brightness Mode
ledHardValue = rgbEncode(2,0,2);
ledMediumValue = rgbEncode(0,2,2);
ledSoftValue = rgbEncode(0,0,2);
ledRedValue = rgbEncode(2,0,0);
setLED(FL_LED, D_HARD);
setLED(FR_LED, D_MEDIUM);
setLED(RL_LED, D_SOFT);
setLED(RR_LED, D_RED);
break;
case LED_LEVEL2:
//SET the LEDs to 2nd to Minimum Brightness Mode
ledHardValue = rgbEncode(10,0,10);
ledMediumValue = rgbEncode(0,10,5);
ledSoftValue = rgbEncode(0,0,10);
ledRedValue = rgbEncode(10,0,0);
setLED(FL_LED, D_HARD);
setLED(FR_LED, D_MEDIUM);
setLED(RL_LED, D_SOFT);
setLED(RR_LED, D_RED);
break;
case LED_LEVEL3:
//SET the LEDs to Medium Brightness Mode
ledHardValue = rgbEncode(70,0,70);
ledMediumValue = rgbEncode(0,70,35);
ledSoftValue = rgbEncode(0,0,70);
ledRedValue = rgbEncode(70,0,0);
setLED(FL_LED, D_HARD);
setLED(FR_LED, D_MEDIUM);
setLED(RL_LED, D_SOFT);
setLED(RR_LED, D_RED);
break;
case LED_LEVEL4:
//SET the LEDs to Maximum Brightness Mode
ledHardValue = rgbEncode(120,0,120);
ledMediumValue = rgbEncode(0,120,60);
ledSoftValue = rgbEncode(0,0,150);
ledRedValue = rgbEncode(120,0,0);
setLED(FL_LED, D_HARD);
setLED(FR_LED, D_MEDIUM);
setLED(RL_LED, D_SOFT);
setLED(RR_LED, D_RED);
break;
}
}
//*************************************************************************************
//
//
// setSTARTUP()
//
//
//************************************************************************************
void setStartup(){
int smode;
//
// Save current controller mode
//
eepromWriteInt(EPROM_CONTROLLER_MODE,previousMode);
/*
Serial.print(F(" Saving Controller Mode ="));
Serial.print(previousMode);
*/
//
// Save current current controller strut mode
//
smode = myController.strutMode();
eepromWriteInt(EPROM_STRUT_MODE,smode);
/*
Serial.print(F(" Saving Strut Mode ="));
Serial.println(smode);
*/
//
// Restore the previous state of the controller
//
myController.setMode(previousMode);
}
//*************************************************************************************
//
//
// setStruts()
//
//
//************************************************************************************
void setStruts(){
for(int strut = 0; strut < 4; strut++){
if(logMode) doLogMode(LOG_STRUTS_NAME , strut,0);
setStrutMode(strut);
}
}
//*************************************************************************************
//
//
// setManualStruts()
//
//
//************************************************************************************
void setManualStruts(){
for(int strut = 0; strut < 4; strut++){
setStrutDirectMode( strut, myStruts[strut].desiredState(), DONT_CLEAR);
}
}
//*************************************************************************************
//
//
// setManualStrutsTargets()
//
//
//************************************************************************************
void setManualStrutTargets(){
for(int strut = 0; strut < 4; strut++){
//
// See if we are changing states
//
if (myStruts[strut].desiredState() != myController.strutMode()){
//
// Yes the human wants a new state - put it in motion
//
myStruts[strut].setDesiredState(myController.strutMode());
myStruts[strut].setTransitionStatus(CHANGING);
myStruts[strut].setLastChangeTime();
myStruts[strut].clearTryCount();
myStruts[strut].setErrorStatus(NO_FAILURES);
}else {
//
// No Change - do nothing
//
}
}
}
//*************************************************************************************
//
//
// setAutoStrutsTargets()
//
//
//************************************************************************************
void setAutoStrutTargets(){
for(int strut = 0; strut < 4; strut++){
//
// See if we are changing states
//
if (myStruts[strut].desiredState() != accelerometerStrutTargets[strut]){
//
// Yes the accelerometer readings dictate a new state - put it in motion
//
myStruts[strut].setDesiredState(accelerometerStrutTargets[strut]);
myStruts[strut].setTransitionStatus(CHANGING);
myStruts[strut].clearTryCount();
myStruts[strut].setErrorStatus(NO_FAILURES);
}else {
//
// No Change - do nothing
//
}
}
}
//*******************************************************************************************
//
//
// Rountine to only allow a strut downward change to Medium or Soft
// until after the hold interval has passed
//
//
//*******************************************************************************************
void setAutoStrutState(int strut, int mode){
unsigned long currentTime = millis();
if ( mode == SOFT && myController.strutMode() == MEDIUM) mode = MEDIUM;
if (mode == HARD){
accelerometerStrutTargets[strut] = mode;
myStruts[strut].setLastChangeTime();
} else if (mode == MEDIUM){
if( accelerometerStrutTargets[strut] == HARD && currentTime < (myStruts[strut].lastChangeTime() + STRUT_HOLD_INTERVAL)) return;
accelerometerStrutTargets[strut] = MEDIUM;
myStruts[strut].setLastChangeTime();
} else if (mode == SOFT){
if( (accelerometerStrutTargets[strut] == HARD ||accelerometerStrutTargets[strut] == MEDIUM)
&& currentTime < (myStruts[strut].lastChangeTime() + STRUT_HOLD_INTERVAL)) return;
accelerometerStrutTargets[strut] = SOFT;
myStruts[strut].setLastChangeTime();
}
}
//*******************************************************************************************
//
//
// Rountine to read current Accelerometer values and set Strut Targets
//
//
//*******************************************************************************************
void setAccelerometerTargets(){
/*
ForwardAcceleration = 0.0;
LeftRightAcceleration = 0.0;
UpDownAcceleration = 0.0;
STRUT_FL
STRUT_FR
STRUT_RL
STRUT_RR
*/
/*
Serial.print(F("Forward Accleration ="));
Serial.print(ForwardAcceleration );
Serial.print(F(" Left Right Accleration ="));
Serial.println(LeftRightAcceleration );
*/
//
// Check to see if we doing NO Acceleration or Braking ?
//
if (ForwardAcceleration > (-1)*mediumThreshold && ForwardAcceleration < mediumThreshold ){
//
// No Foward or Braking Acceleration
//
/*
Serial.println(F("No Forward or Braking Acceleration"));
*/
if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration <mediumThreshold ) {
//
// No lateral Acceleration - set all struts to soft
//
setAutoStrutState(STRUT_FL, SOFT);
setAutoStrutState(STRUT_FR, SOFT);
setAutoStrutState(STRUT_RL, SOFT);
setAutoStrutState(STRUT_RR, SOFT);
}else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration <hardThreshold ) {
//
// Doing a MEDIUM Left Turn at same time
// Set right side struts to medium &
//
setAutoStrutState(STRUT_FL, SOFT);
setAutoStrutState(STRUT_FR, MEDIUM);
setAutoStrutState(STRUT_RL, SOFT);
setAutoStrutState(STRUT_RR, MEDIUM);
}else if (LeftRightAcceleration >= hardThreshold ) {
//
// Doing a HARD Left Turn at same time-
// set rear struts to SOFT & right side struts to Hard
//
setAutoStrutState(STRUT_FL, SOFT);
setAutoStrutState(STRUT_FR, HARD);
setAutoStrutState(STRUT_RL, SOFT);
setAutoStrutState(STRUT_RR, HARD);
}else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
//
// Doing a MEDIUM RIGHT Turn at same time-
// set rear to SOFT & left side struts to medium &
//
setAutoStrutState(STRUT_FL, MEDIUM);
setAutoStrutState(STRUT_FR, SOFT);
setAutoStrutState(STRUT_RL, MEDIUM);
setAutoStrutState(STRUT_RR, SOFT);
}else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
//
// Doing a HARD RIGHT Turn at same time-
// set rear struts to SOFT & left side struts to Hard
//
setAutoStrutState(STRUT_FL, HARD);
setAutoStrutState(STRUT_FR, SOFT);
setAutoStrutState(STRUT_RL, HARD);
setAutoStrutState(STRUT_RR, SOFT);
}
//
//Check to see if we are doing MEDIUM range Forward Acceleration ?
//
} else if(ForwardAcceleration >= mediumThreshold && ForwardAcceleration < hardThreshold ){
//
// Doing MEDIUM Forward Acceleration between mediumThresholdg's and hardThreshold
// In this range the Rear Struts will be set to Medium Mode
// And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
//
/*
Serial.println(F("MEDIUM Forward Acceleration"));
*/
if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration <mediumThreshold ) {
//
// No lateral Acceleration - set just rear struts to medium
//
setAutoStrutState(STRUT_FL, SOFT);
setAutoStrutState(STRUT_FR, SOFT);
setAutoStrutState(STRUT_RL, MEDIUM);
setAutoStrutState(STRUT_RR, MEDIUM);
}else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration <hardThreshold ) {
//
// Doing a MEDIUM Left Turn at same time
// Set RIGHT side struts to medium &
//
setAutoStrutState(STRUT_FL, SOFT);
setAutoStrutState(STRUT_FR, MEDIUM);
setAutoStrutState(STRUT_RL, MEDIUM);
setAutoStrutState(STRUT_RR, MEDIUM);
}else if (LeftRightAcceleration >= hardThreshold ) {
//
// Doing a HARD Left Turn at same time-
// set RIGHT side struts to Hard
//
setAutoStrutState(STRUT_FL, SOFT);
setAutoStrutState(STRUT_FR, HARD);
setAutoStrutState(STRUT_RL, MEDIUM);
setAutoStrutState(STRUT_RR, HARD);
}else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
//
// Doing a MEDIUM RIGHT Turn at same time-
// set LEFT side struts to medium
//
setAutoStrutState(STRUT_FL, MEDIUM);
setAutoStrutState(STRUT_FR, SOFT);
setAutoStrutState(STRUT_RL, MEDIUM);
setAutoStrutState(STRUT_RR, MEDIUM);
}else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
//
// Doing a HARD RIGHT Turn at same time-
// set LEFT side struts to Hard
//
setAutoStrutState(STRUT_FL, HARD);
setAutoStrutState(STRUT_FR, SOFT);
setAutoStrutState(STRUT_RL, HARD);
setAutoStrutState(STRUT_RR, MEDIUM);
}
//
//Check to see if we are doing Hard range Forward Acceleration ?
//
} else if(ForwardAcceleration >= hardThreshold ){
//
// Doing HARD Forward Acceleration greater than hardThresholdgs
// In this range the Rear Struts will be set to HARD Mode
// And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
//
/*
Serial.println(F("HARD Forward Acceleration"));
*/
if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration <mediumThreshold ) {
//
// No lateral Acceleration - set just rear struts to HARD
//
setAutoStrutState(STRUT_FL, SOFT);
setAutoStrutState(STRUT_FR, SOFT);
setAutoStrutState(STRUT_RL, HARD);
setAutoStrutState(STRUT_RR, HARD);
}else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration <hardThreshold ) {
//
// Doing a MEDIUM Left Turn at same time
// Set right side struts to medium
//
setAutoStrutState(STRUT_FL, SOFT);
setAutoStrutState(STRUT_FR, MEDIUM);
setAutoStrutState(STRUT_RL, HARD);
setAutoStrutState(STRUT_RR, HARD);
}else if (LeftRightAcceleration >= hardThreshold ) {
//
// Doing a HARD Left Turn at same time-
// set rear struts to medium & right side struts to Hard
//
setAutoStrutState(STRUT_FL, SOFT);
setAutoStrutState(STRUT_FR, HARD);
setAutoStrutState(STRUT_RL, HARD);
setAutoStrutState(STRUT_RR, HARD);
}else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
//
// Doing a MEDIUM RIGHT Turn at same time-
// set rear & left side struts to medium &
//
setAutoStrutState(STRUT_FL, MEDIUM);
setAutoStrutState(STRUT_FR, SOFT);
setAutoStrutState(STRUT_RL, HARD);
setAutoStrutState(STRUT_RR, HARD);
}else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
//
// Doing a HARD RIGHT Turn at same time-
// set rear struts to medium & left side struts to Hard
//
setAutoStrutState(STRUT_FL, HARD);
setAutoStrutState(STRUT_FR, SOFT);
setAutoStrutState(STRUT_RL, HARD);
setAutoStrutState(STRUT_RR, HARD);
}
//
//Check to see if we are doing MEDIUM range Braking
//
} else if(ForwardAcceleration > (-1)*hardThreshold && ForwardAcceleration < (-1)*mediumThreshold){
//
// Doing MEDIUM range Braking between 2.0 & 4.0 g's
// In this range the Front Struts will be set to MEDIUM Mode
// And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
//
/*
* Serial.println(F("MEDIUM Braking"));
*/
if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration <mediumThreshold ) {
//
// No lateral Acceleration - set just Front struts to HARD
//
setAutoStrutState(STRUT_FL, MEDIUM);
setAutoStrutState(STRUT_FR, MEDIUM);
setAutoStrutState(STRUT_RL, SOFT);
setAutoStrutState(STRUT_RR, SOFT);
}else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration <hardThreshold ) {
//
// Doing a MEDIUM Left Turn at same time
// Set right side struts to medium
//
setAutoStrutState(STRUT_FL, MEDIUM);
setAutoStrutState(STRUT_FR, MEDIUM);
setAutoStrutState(STRUT_RL, SOFT);
setAutoStrutState(STRUT_RR, MEDIUM);
}else if (LeftRightAcceleration >= hardThreshold ) {
//
// Doing a HARD Left Turn at same time-
// Set right side struts to Hard
//
setAutoStrutState(STRUT_FL, MEDIUM);
setAutoStrutState(STRUT_FR, HARD);
setAutoStrutState(STRUT_RL, SOFT);
setAutoStrutState(STRUT_RR, HARD);
}else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
//
// Doing a MEDIUM RIGHT Turn at same time-
// Set left side struts to medium &
//
setAutoStrutState(STRUT_FL, MEDIUM);
setAutoStrutState(STRUT_FR, MEDIUM);
setAutoStrutState(STRUT_RL, MEDIUM);
setAutoStrutState(STRUT_RR, SOFT);
}else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
//
// Doing a HARD RIGHT Turn at same time-
// left side struts to Hard
//
setAutoStrutState(STRUT_FL, HARD);
setAutoStrutState(STRUT_FR, MEDIUM);
setAutoStrutState(STRUT_RL, HARD);
setAutoStrutState(STRUT_RR, SOFT);
}
//
//Check to see if we are doing HARD range Braking
//
} else if(ForwardAcceleration <= (-1)*hardThreshold ){
//
// Doing HARD range Braking greater than 4.0 g's
// In this range the Front Struts will be set to Hard Mode
// And any Lateral accelerations due to turns will set the LEFT or RIGHT Struts
//
/*
Serial.println(F("Hard Braking"));
*/
if (LeftRightAcceleration > (-1)*mediumThreshold && LeftRightAcceleration <mediumThreshold ) {
//
// No lateral Acceleration - set just Front struts to HARD
//
setAutoStrutState(STRUT_FL, HARD);
setAutoStrutState(STRUT_FR, HARD);
setAutoStrutState(STRUT_RL, SOFT);
setAutoStrutState(STRUT_RR, SOFT);
}else if (LeftRightAcceleration >= mediumThreshold && LeftRightAcceleration <hardThreshold ) {
//
// Doing a MEDIUM Left Turn at same time
// Set right side struts to medium
//
setAutoStrutState(STRUT_FL, HARD);
setAutoStrutState(STRUT_FR, HARD);
setAutoStrutState(STRUT_RL, SOFT);
setAutoStrutState(STRUT_RR, MEDIUM);
}else if (LeftRightAcceleration >= hardThreshold ) {
//
// Doing a HARD Left Turn at same time-
// Set right side struts to Hard
//
setAutoStrutState(STRUT_FL, HARD);
setAutoStrutState(STRUT_FR, HARD);
setAutoStrutState(STRUT_RL, SOFT);
setAutoStrutState(STRUT_RR, HARD);
}else if (LeftRightAcceleration > (-1)*hardThreshold && LeftRightAcceleration <= (-1)*mediumThreshold ) {
//
// Doing a MEDIUM RIGHT Turn at same time-
// Set left side struts to medium &
//
setAutoStrutState(STRUT_FL, HARD);
setAutoStrutState(STRUT_FR, HARD);
setAutoStrutState(STRUT_RL, MEDIUM);
setAutoStrutState(STRUT_RR, SOFT);
}else if (LeftRightAcceleration <= (-1)*hardThreshold ) {
//
// Doing a HARD RIGHT Turn at same time-
// left side struts to Hard
//
setAutoStrutState(STRUT_FL, HARD);
setAutoStrutState(STRUT_FR, HARD);
setAutoStrutState(STRUT_RL, HARD);
setAutoStrutState(STRUT_RR, SOFT);
}
}
}
///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to make one pass thru a Strut to Set the desired strut to a particular mode
// this routine must be called in a loop
//
/////////////////////////////////////////////////////////////////////////////////////
void setStrutMode( int strut ){
int maxTries;
int currentState;
unsigned long currentTime;
currentTime = millis();
if (myController.mode() == MANUAL) maxTries = MANUAL_MAXTRIES;
else if (myController.mode() == AUTO) maxTries = AUTO_MAXTRIES;
//
// If this strut has exceeded maximum allowed failures bail
//
if (myStruts[strut].failures() > MAXFAILURES){
// Make sure Motor is Off
myStruts[strut].motorOff();
if(logMode){
Serial.print("MAXFAILS,");
Serial.print("FAILED,");
Serial.println("FAILED");
}
return;
}
//
// Jump to strut state
//
switch (myStruts[strut].transitionStatus()){
case OLDSTRUTS:
//
// This case deals with old struts where the motors & mechansim
// can cause the strut to have problems getting into mode
// This state will purposely run this strut motor
// for an additional 20msec to try an force it into mode
// turn off the motors and then see if the strut is in mode
// and then set state accordingly
if( currentTime < myStruts[strut].lastChangeTime() + 20) break;
//
// OK the motors for this strut have been forced on for 20 msecs
// Turn them off or make sure their still off.
myStruts[strut].motorOff();
//
// Wait for an additional 10 msecs( 20+10) for strut & switches to stablize
//
if( currentTime < myStruts[strut].lastChangeTime() + 30) break;
currentState = myStruts[strut].actualState(ERROR);
if( currentState == myStruts[strut].desiredState() ){
// Ok Struts have held desired mode for 5 msecs
// Switch to Nominal
myStruts[strut].setErrorStatus(NO_FAILURES);
myStruts[strut].setTransitionStatus(NOMINAL);
myStruts[strut].setLastChangeTime();
}else{
//
// Ok we were not able to get the struts into the correct
// mode using old struts mode
// flag them as bad
//
myStruts[strut].motorOff();
myStruts[strut].setLastChangeTime();
myStruts[strut].setTransitionStatus(FAILED);
myStruts[strut].setErrorStatus(MOTOR_COMMAND_FAILURE);
myStruts[strut].incrementFailures();
setLED(strut,D_GREENYELLOW);
}
break;
case PRENOMINAL:
//
// This state is used just after a strut has indicated it
// has achieved the desired mode
// it will double check the state of after 5 msec
// and declare it Nominal if it stays in the desired mode
//
//
// wait 10 msec before checking status
//
if(currentTime < myStruts[strut].lastChangeTime() + 10) break;
currentState = myStruts[strut].actualState(ERROR);
if( currentState == myStruts[strut].desiredState() ){
// Ok Struts have held desired mode
// Switch to Nominal
myStruts[strut].setErrorStatus(NO_FAILURES);
myStruts[strut].setTransitionStatus(NOMINAL);
myStruts[strut].setLastChangeTime();
}else{
//
// Ok it looks like we might be dealing with old struts
// turn the motors back on and set to Old Struts mode
//
myStruts[strut].motorOn();
myStruts[strut].incrementTryCount();
myStruts[strut].setLastChangeTime();
myStruts[strut].setTransitionStatus(OLDSTRUTS);
}
break;
case FAILED:
if (myStruts[strut].errorStatus() == SIGNAL_WIRE_FAILURE){
setLED(strut,D_YELLOW);
}else if (myStruts[strut].errorStatus() == MOTOR_COMMAND_FAILURE){
setLED(strut,D_RED);
}
break;
case NOMINAL:
currentState = myStruts[strut].actualState(ERROR);
if (currentState != myStruts[strut].desiredState()) {
setLED(strut,D_YELLOW);
myStruts[strut].setErrorStatus(SIGNAL_WIRE_FAILURE);
myStruts[strut].setTransitionStatus(FAILED);
myStruts[strut].incrementFailures();
}
break;
case CHANGING:
currentState = myStruts[strut].actualState(ERROR);
if( myStruts[strut].tryCount() < maxTries ){
if( currentState == myStruts[strut].desiredState() ){
//
// Ok.. we just got the strut into the desired state
//
//
// make sure the motors are off
myStruts[strut].motorOff();
// Strut looks like it is now in desired mode
// change state to PRENOMINAL - that state will figure out if strut
// really stayed in desired state
//
myStruts[strut].setErrorStatus(NO_FAILURES);
myStruts[strut].setTransitionStatus(PRENOMINAL);
myStruts[strut].setLastChangeTime();
}else {
//
// Ok still not in correct state keep motors on
myStruts[strut].motorOn();
myStruts[strut].incrementTryCount();
}
}else {
//
// Maxtries was exceeded - display RED
//
// Turn the Motor Off
myStruts[strut].motorOff();
setLED(strut,D_RED);
myStruts[strut].setErrorStatus(MOTOR_COMMAND_FAILURE);
myStruts[strut].setTransitionStatus(FAILED);
myStruts[strut].incrementFailures();
}
break;
}
if(logMode){
if (myStruts[strut].errorStatus() == SIGNAL_WIRE_FAILURE){
Serial.print("WIRE_FAIL,");
}else if (myStruts[strut].errorStatus() == MOTOR_COMMAND_FAILURE){
Serial.print("MOTOR_FAIL,");
}else if (myStruts[strut].errorStatus() == NO_FAILURES){
Serial.print("STRUT_OK,");
}
if (myStruts[strut].desiredState() == HARD){
Serial.print("T=HARD,");
}else if (myStruts[strut].desiredState() == MEDIUM){
Serial.print("T=MEDIUM,");
}else if (myStruts[strut].desiredState() == SOFT){
Serial.print("T=SOFT,");
}
if(strut == 3){
if (currentState == HARD){
Serial.println("HARD");
}else if (currentState == MEDIUM){
Serial.println("MEDIUM");
}else if (currentState == SOFT){
Serial.println("SOFT");
}else Serial.println("Unknown");
}else{
if (currentState == HARD){
Serial.print("HARD, ");
}else if (currentState == MEDIUM){
Serial.print("MEDIUM, ");
}else if (currentState == SOFT){
Serial.print("SOFT, ");
}else Serial.print("Unknown, ");
}
}
}
///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to Read actual status of strut , set LED color apprropriately and return value
//
//
///////////////////////////////////////////////////////////////////////////////////////
int readStrutState( int strut, int displayMode){
int strutS1;
int strutS2;
int switch1;
int switch2;
int switch1b;
int switch2b;
int timeout = 0;
int numberTries = 0;
/*
unsigned long timestart;
unsigned long timenow;
timestart = millis();
*/
strutS1 = myStruts[strut].switch1();
strutS2 = myStruts[strut].switch2();
//
// intial switch reads
//
switch1 = digitalRead(strutS1);
switch2 = digitalRead(strutS2);
delayMicroseconds(50); //Wait for 100us to debounce contacts
switch1b = digitalRead(strutS1);
switch2b = digitalRead(strutS2);
//
// See if the switches are reading the same
//
while ((switch2 != switch2b || switch1 != switch1b)&& numberTries < 40){
switch1 = digitalRead(strutS1);
switch2 = digitalRead(strutS2);
delayMicroseconds(50); //Wait for 50us to debounce contacts
switch1b = digitalRead(strutS1);
switch2b = digitalRead(strutS2);
numberTries++;
}
if (switch2 == HIGH && switch1 == LOW && numberTries <40 ){
// Strut is in HARD mode
setLED(strut, D_HARD);
return(HARD);
}else if (switch2 == LOW && switch1 == LOW && numberTries <40 ){
// If Strut is in MEDIUM mode
setLED(strut, D_MEDIUM);
return(MEDIUM);
}else if (switch2 == LOW && switch1 == HIGH && numberTries <40 ){
// Strut is in SOFT Mode
setLED(strut, D_SOFT);
return(SOFT);
}else {
// setLED(strut, D_UNKNOWN);
setLED(strut, D_OFF);
return(UNKNOWN);
}
}
///////////////////////////////////////////////////////////////////////////////////////
//
//
// Routine to set a Strut to a desired mode
// this routine has it's own internal loop
//
/////////////////////////////////////////////////////////////////////////////////////
void setStrutDirectMode( int strut, int Mode,int Clear ){
int currentState;
int maxTries = 20000;
/*
SIGNAL_WIRE_FAILURE
MOTOR_COMMAND_FAILURE
NO_FAILURES
if (myStruts[strut].desiredState() != myController.strutMode()){*
myStruts[strut].setErrorStatus(NO_FAILURES);
myStruts[strut].setDesiredState(myController.strutMode());
myStruts[strut].setTransitionStatus(CHANGING);
STRUT_FL 0
STRUT_FR 1
STRUT_RL 2
STRUT_RR 3
*/
if(Clear == CLEAR){
myStruts[strut].clearFailures();
myStruts[strut].clearTryCount();
myStruts[strut].setErrorStatus(NO_FAILURES);
}
myStruts[strut].setDesiredState(Mode);
myStruts[strut].setTransitionStatus(CHANGING);
while (myStruts[strut].transitionStatus() == CHANGING ){
if (myStruts[strut].failures() > MAXFAILURES){
// Make sure Motor is Off
myStruts[strut].motorOff();
return;
}
switch (myStruts[strut].transitionStatus()){
case FAILED:
setLED(strut,D_RED);
return;
break;
case NOMINAL:
currentState = myStruts[strut].actualState(ERROR);
if (currentState != myStruts[strut].desiredState()) {
setLED(strut,D_YELLOW);
myStruts[strut].setErrorStatus(SIGNAL_WIRE_FAILURE);
myStruts[strut].setTransitionStatus(FAILED);
myStruts[strut].incrementFailures();
}
return;
case CHANGING:
currentState = myStruts[strut].actualState(ERROR);
if( myStruts[strut].tryCount() < maxTries ){
if( currentState == myStruts[strut].desiredState() ){
//
// Ok.. we just got the strut into the desired state
// delay for X milliseconds to make sure into mode.
// delay(30);
// Turn the Motor Off
myStruts[strut].motorOff();
// Strut is now in desired mode
myStruts[strut].setErrorStatus(NO_FAILURES);
myStruts[strut].setTransitionStatus(NOMINAL);
//
// Pause for 1 second and do a final check to see if strut stayed in the desired mode
//
delay(1000);
currentState = myStruts[strut].actualState(ERROR);
if (currentState != myStruts[strut].desiredState()) {
setLED(strut,D_YELLOW);
myStruts[strut].setErrorStatus(SIGNAL_WIRE_FAILURE);
myStruts[strut].setTransitionStatus(FAILED);
myStruts[strut].incrementFailures();
}
}else {
//
// Ok still not in correct state keep motors on
myStruts[strut].motorOn();
myStruts[strut].incrementTryCount();
}
}else {
//
// Maxtries was exceeded - display RED
//
// Turn the Motor Off
myStruts[strut].motorOff();
setLED(strut,D_RED);
myStruts[strut].setErrorStatus(MOTOR_COMMAND_FAILURE);
myStruts[strut].setTransitionStatus(FAILED);
myStruts[strut].incrementFailures();
}
break;
}
}
}
//*********************************************************************************************
//
//
// Logging Routine
//
//
//*********************************************************************************************
void doLogMode(int mode,int val1,int val2){
unsigned long currentTime = millis();
/*
* Time, Controller Mode, Controller Target , Acc FWD Avg, Acc LeftRight Avg , Acc UP Avg,
* Strut Name, FL Strut STatus, FL Target, FL Strut Actual
* Strut Name, FR Strut STatus, FR Target, FR Strut Actual
* Strut Name, RL Strut STatus, RL Target, RL Strut Actual
* Strut Name, RR Strut STatus, RR Target, RR Strut Actual
*
*/
switch (mode){
case LOG_TIME:
Serial.print(currentTime);
Serial.print(F(","));
if(myController.mode() == MANUAL){
Serial.print(F("MANUAL,"));
}else if(myController.mode() == AUTO){
Serial.print(F("AUTO,"));
}else{
Serial.print(F("MODE?,"));
}
if (myController.strutMode() == HARD){
Serial.print(F("HARD,"));
}else if (myController.strutMode() == MEDIUM){
Serial.print(F("MEDIUM,"));
}else if (myController.strutMode() == SOFT){
Serial.print(F("SOFT,"));
}
break;
case LOG_STRUTS_NAME:
if (val1 == 0) Serial.print("FL,");
else if (val1 == 1) Serial.print("FR,");
else if (val1 == 2) Serial.print("RL,");
else if (val1 == 3) Serial.print("RR,");
break;
case LOG_ACCELEROMETER:
readAccelerometerValues();
break;
}
}
//**************************************************************************
// this routine converts three seperate RGB values in to a correctly encoded RGB integer
//**************************************************************************
long rgbEncode(int red,int green,int blue){
long rgb;
//if (red > 255) red=255;
//if (green > 255) green=255;
//if (blue > 255) blue=255;
rgb = red*65536 + green*256 + blue;
return (rgb);
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Blink one of the lights
///////////////////////////////////////////////////////////////////////////////////////
void blinkLights(int lightaddress, int numberBlinks,int blinkDelay,int numberGroupBlinks,int groupDelay){
setLights(lightaddress,LOW);
for (int groupcount = numberGroupBlinks; groupcount > 0; groupcount--){
for (int count = numberBlinks; count > 0; count--){
delay(blinkDelay);
setLights(lightaddress,HIGH);
delay(blinkDelay);
setLights(lightaddress,LOW);
}
delay(groupDelay);
}
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Blink Both of the lights at the same time
///////////////////////////////////////////////////////////////////////////////////////
void blinkBothLights(int numberBlinks,int blinkDelay,int numberGroupBlinks,int groupDelay,int doLed){
// Turn off both Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
for (int groupcount = numberGroupBlinks; groupcount > 0; groupcount--){
for (int count = numberBlinks; count > 0; count--){
delay(blinkDelay);
setLights(SportLight,HIGH);
setLights(TourLight,HIGH);
if(doLed){
setLED(STRUT_FL,D_WHITE);
setLED(STRUT_FR,D_WHITE);
}
delay(blinkDelay);
setLights(SportLight,LOW);
setLights(TourLight,LOW);
if(doLed){
setLED(STRUT_FL,D_OFF);
setLED(STRUT_FR,D_OFF);
}
}
delay(groupDelay);
}
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to Set TourSport Lights during command loop
///////////////////////////////////////////////////////////////////////////////////////
void setLoopTourSportLights(){
int strutMode;
unsigned long currentTime = millis();
if(currentTime <= lightCycleStartTime + 30000){
//
// time to do Standard Lights Status
//
if(myController.mode() == MANUAL) strutMode = myController.strutMode();
else if (myController.mode() == AUTO) {
strutMode = SOFT;
for(int strut = 0; strut < 4; strut++){
if(myStruts[strut].desiredState() == MEDIUM && strutMode == SOFT ) strutMode = MEDIUM;
if(myStruts[strut].desiredState() == HARD && ( strutMode == MEDIUM || strutMode == SOFT)) strutMode = HARD;
}
}
switch (strutMode){
case(HARD):
setLights(SportLight,HIGH);
setLights(TourLight,LOW);
break;
case(MEDIUM):
setLights(SportLight,HIGH);
setLights(TourLight,HIGH);
break;
case(SOFT):
setLights(SportLight,LOW);
setLights(TourLight,HIGH);
break;
case(UNKNOWN):
setLights(SportLight,LOW);
setLights(TourLight,LOW);
break;
}
signalWireFailureCycleStartTime = millis();
}else if(lightCycleStartTime+30000 < currentTime && currentTime <= lightCycleStartTime + 31500){
//
// time to do Signal Wire Status
//
// if there was a problem start flashing shit.. to the do sport/tourlight error thing
if (myStruts[STRUT_FL].errorStatus() == SIGNAL_WIRE_FAILURE ||
myStruts[STRUT_FR].errorStatus() == SIGNAL_WIRE_FAILURE ||
myStruts[STRUT_RL].errorStatus() == SIGNAL_WIRE_FAILURE ||
myStruts[STRUT_RR].errorStatus() == SIGNAL_WIRE_FAILURE ) {
if(currentTime < signalWireFailureCycleStartTime + 250 ){
setLights(SportLight,LOW);
setLights(TourLight,HIGH);
}else if(currentTime < signalWireFailureCycleStartTime + 500 ){
setLights(SportLight,HIGH);
setLights(TourLight,LOW);
}else if(currentTime < signalWireFailureCycleStartTime + 750 ){
setLights(SportLight,LOW);
setLights(TourLight,HIGH);
}else if(currentTime < signalWireFailureCycleStartTime + 1000 ){
setLights(SportLight,HIGH);
setLights(TourLight,LOW);
}else if(currentTime < signalWireFailureCycleStartTime + 1250 ){
setLights(SportLight,LOW);
setLights(TourLight,HIGH);
}else if(currentTime < signalWireFailureCycleStartTime + 1500 ){
setLights(SportLight,HIGH);
setLights(TourLight,LOW);
}
}
commandFailureCycleStartTime = millis();
}else if(lightCycleStartTime+31500 < currentTime && currentTime <= lightCycleStartTime + 33000){
//
// time to do Motor Command Status
//
// if there was a problem start flashing shit.. to the do sport/tourlight error thing
if (myStruts[STRUT_FL].errorStatus() == MOTOR_COMMAND_FAILURE ||
myStruts[STRUT_FR].errorStatus() == MOTOR_COMMAND_FAILURE ||
myStruts[STRUT_RL].errorStatus() == MOTOR_COMMAND_FAILURE ||
myStruts[STRUT_RR].errorStatus() == MOTOR_COMMAND_FAILURE ) {
if(currentTime < commandFailureCycleStartTime + 250 ){
setLights(SportLight,LOW);
setLights(TourLight,LOW);
}else if(currentTime < commandFailureCycleStartTime + 500 ){
setLights(SportLight,HIGH);
setLights(TourLight,HIGH);
}else if(currentTime < commandFailureCycleStartTime + 750 ){
setLights(SportLight,LOW);
setLights(TourLight,LOW);
}else if(currentTime < commandFailureCycleStartTime + 1000 ){
setLights(SportLight,HIGH);
setLights(TourLight,HIGH);
}else if(currentTime < commandFailureCycleStartTime + 1250 ){
setLights(SportLight,LOW);
setLights(TourLight,LOW);
}else if(currentTime < commandFailureCycleStartTime + 1500 ){
setLights(SportLight,HIGH);
setLights(TourLight,HIGH);
}
}
} else if (currentTime > lightCycleStartTime + 33000) lightCycleStartTime = millis();
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to intialiaze IOPins for Strut Motors
///////////////////////////////////////////////////////////////////////////////////////
void initializeMotorIOPin(int pin){
// Set pin used for strut motor as output
pinMode(pin, OUTPUT);
// Make sure Motor is off
digitalWrite(pin,LOW);
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to intialiaze IOPins for Strut Switches
///////////////////////////////////////////////////////////////////////////////////////
void initializeStrutIOPin(int pin){
// Set pin used for strut motor as output
pinMode(pin, INPUT);
// Pull Pin HIGH
digitalWrite(pin,HIGH);
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to set an LED to a color by Address
///////////////////////////////////////////////////////////////////////////////////////
void setLED(int ledaddress, int color){
//
// rgbEncode(int red,int green,int blue)
// red,green,blue = 0, 255
//
switch(color){
case D_HARD:
setLedColors(ledaddress,ledHardValue);
break;
case D_MEDIUM:
setLedColors(ledaddress,ledMediumValue);
break;
case D_SOFT:
setLedColors(ledaddress,ledSoftValue);
break;
case D_RED:
setLedColors(ledaddress,ledRedValue);
break;
case D_GREEN:
setLedColors(ledaddress,rgbEncode(0,10,0));
break;
case D_BLUE:
setLedColors(ledaddress,rgbEncode(0,0,25));
break;
case D_TURQUOIS:
setLedColors(ledaddress,rgbEncode(0,40,64));
break;
case D_PURPLE:
setLedColors(ledaddress,rgbEncode(10,0,10));
break;
case D_YELLOW:
setLedColors(ledaddress,rgbEncode(15,15,0));
break;
case D_GREENYELLOW:
setLedColors(ledaddress,rgbEncode(55,128,0));
break;
case D_STARTUP:
setLedColors(ledaddress,rgbEncode(2,2,2));
break;
case D_WHITE:
setLedColors(ledaddress,rgbEncode(15,15,15));
break;
case D_OFF:
setLedColors(ledaddress,rgbEncode(0,0,0));
break;
case D_UNKNOWN:
setLedColors(ledaddress,rgbEncode(53,18,0));
break;
}
// delayMicroseconds(600); //Wait for 200us to go into reset
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to turn an LED off
//////////////////////////////////////////////////////////////////////////////////////
void offLED(int ledaddress){
setLedColors(ledaddress,LEDOFF);
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to set either the Tour Or Sport light to an HIGH/ON or LOW/OFF state
///////////////////////////////////////////////////////////////////////////////////////
void setLights(int light, int state){
digitalWrite(light,state);
}
///////////////////////////////////////////////////////////////////////////////////////
// Routine to turn off all strut motors
///////////////////////////////////////////////////////////////////////////////////////
void motorsOff(){
myStruts[0].motorOff();
myStruts[1].motorOff();
myStruts[2].motorOff();
myStruts[3].motorOff();
}
//****************************************************
//
// Convert Desired HEX color into rgb values and store in LED array
//
//****************************************************
void setLedColors(int LED,long hexValue) {
if( ledCurrentValue[LED] == hexValue ) return;
ledCurrentValue[LED] = hexValue ;
ledColors[LED].r = ((hexValue >> 16) & 0xFF) ; // Extract the RR byte
ledColors[LED].g = ((hexValue >> 8) & 0xFF) ; // Extract the GG byte
ledColors[LED].b = ((hexValue) & 0xFF) ; // Extract the BB byte
ledColors[LED+4].r = ledColors[LED].r ;
ledColors[LED+4].g = ledColors[LED].g ;
ledColors[LED+4].b = ledColors[LED].b ;
LEDS.show(); //PUSH the current color definitions of ALL the LEDs out
}
/////////////////////////////////////////////////////////////////////////////////////
//
//
// routine to read the switch pushes while setting LED brightness
//
//
//
/////////////////////////////////////////////////////////////////////////////////////
int readPushButtonSwitch(){
int numberPushes =0;
unsigned long currentTime;
currentTime = millis();
//
// If no presses have occurred or < 0.5 secs since push then return
//
if(pushCount == 0) return(NOCHANGE);
if(currentTime < (pushTime + 1500)) {
/*
Serial.print(F("PushCount < 2000ms ="));
Serial.println(pushCount);
*/
return(NOCHANGE);
}
/*
Serial.print(F("PushCount >2000 ="));
Serial.println(pushCount);
*/
//
// Make sure pushCount is in valid range - if out of range set back to zero
//
if(pushCount > 4){
pushCount = 0;
return (NOCHANGE);
}
numberPushes = pushCount;
pushCount = 0;
return(numberPushes);
/*
* Alternate code for more complex return
//
// Ok button has been pressed one or more times and 0.5 seconds has passed since last push
// now decide what new state we are in
//
switch (pushCount){
case 1:
pushCount = 0;
return(1);
break;
case 2:
pushCount = 0;
return(2);
break;
case 3:
pushCount = 0;
return(3);
break;
case 4:
pushCount = 0;
return(4);
break;
break;
}
*/
}
/////////////////////////////////////////////////////////////////////////////////////
// Reference 5v = 1023
//
// Hard 0.0v / xxx
// Cutoff 0.415 / 85
// Medium 0.83v / xxx
// Cutoff 1.245 / 255
// Soft 1.66v / xxx
// Cutoff 2.075 / 424
// Diag 2.5 / xxx
// Cutoff 2.915 / 596
/////////////////////////////////////////////////////////////////////////////////////
int readControllerSwitch(){
int voltage;
int voltage2;
float voltdiff;
unsigned long currentTime ;
// Based on the selector switch type do the appropriate read
if( myController.selectorType() == ROTARY){
voltage = analogRead(selectorSwitchPin)+1; // read the voltage
delay(50); // debounce for 50 msec
voltage2 = analogRead(selectorSwitchPin)+1;// read the voltage again
voltdiff = abs(1.0-voltage/voltage2); // Calculate the percentage difference
//
// as long as voltage difference > 10% keep reading
//
while(voltdiff > .1 ){
voltage = analogRead(selectorSwitchPin)+1; // read the voltage
delay(50); // debounce for 50 msec
voltage2 = analogRead(selectorSwitchPin)+1;// read the voltage again
voltdiff = abs(1.0-voltage/voltage2); // Calculate the percentage difference
}
//
// based on the converted voltage value and the range return the value
//
if(voltage < 85){
// Selector is in HARD mode
if (myController.strutMode() == HARD) return (NOCHANGE) ;
else {
myController.setMode(MANUAL);
myController.setControllerStrutMode(HARD);
return(HARD);
}
}else if (voltage >= 85 && voltage < 255){
// Selector is in MEDIUM mode
if (myController.strutMode() == MEDIUM) return (NOCHANGE) ;
else {
myController.setMode(MANUAL);
myController.setControllerStrutMode(MEDIUM);
return(MEDIUM);
}
}else if (voltage >= 255 && voltage < 424){
// Selector is in SOFT mode
if (myController.strutMode() == SOFT) return (NOCHANGE) ;
else {
myController.setMode(MANUAL);
myController.setControllerStrutMode(SOFT);
return(SOFT);
}
}else if(voltage >= 424 && voltage <596){
// Selector is in DIAG mode
if (myController.mode() == DIAG) return (NOCHANGE) ;
else {
myController.setMode(DIAG);
return(DIAG);
}
}else{
myController.setMode(UNKNOWN);
return (UNKNOWN);
}
}else if( myController.selectorType() == PUSHBUTTON){
currentTime = millis();
//
// If no presses have occurred or < 1.5 secs since last push then return
//
if(pushCount ==0) return(NOCHANGE);
if(currentTime < (pushTime + 1500)) {
/*
Serial.print(F("PushCount < 2000ms ="));
Serial.println(pushCount);
*/
return(NOCHANGE);
}
/*
Serial.print(F("PushCount >2000 ="));
Serial.println(pushCount);
*/
//
// Make sure pushCount is in valid range - if out of range set back to zero
//
if(pushCount > 9){
pushCount = 0;
return (NOCHANGE);
}
//
// Ok button has been pressed one or more times and 1.5 seconds has passed since last push
// now decide what new state we are in
//
switch (pushCount){
//
// Changing to next strut mode using sequence H -> S -> M -> H -> S -> M ->....
case 1:
pushCount = 0;
//
// If in Diag or readDiag mode and single push occurs treat this as an abort to go back to manual mode
//
if(myController.mode() == DIAG || myController.mode() == READDIAG){
myController.setMode(MANUAL);
return(myController.strutMode());
}
switch(myController.strutMode()){
case HARD:
myController.setControllerStrutMode(SOFT);
return(SOFT);
break;
case MEDIUM:
if(myController.mode() == AUTO) myController.setControllerStrutMode(SOFT);
else myController.setControllerStrutMode(HARD);
return(HARD);
break;
case SOFT:
myController.setControllerStrutMode(MEDIUM);
return(MEDIUM);
break;
}
break;
//
// Changing Controller mode from Auto to Manual or vice versa
case 2:
motorsOff();
pushCount = 0;
switch(myController.mode()){
case MANUAL:
myController.setMode(AUTO); // Change controller to Auto MODE
myController.setControllerStrutMode(SOFT);
//
// Blink both lights alternating(3 times) to indicate auto mode
//
for(int i=0;i<2;i++){
setLights(SportLight,HIGH);
setLED(STRUT_FR,D_WHITE);
setLights(TourLight,LOW);
setLED(STRUT_FL,D_OFF);
delay(250);
setLights(SportLight,LOW);
setLED(STRUT_FR,D_OFF);
setLights(TourLight,HIGH);
setLED(STRUT_FL,D_WHITE);
delay(250);
}
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
setLED(STRUT_FL,D_OFF);
setLED(STRUT_FR,D_OFF);
return(myController.strutMode());
break;
case AUTO:
case DIAG:
case READDIAG:
myController.setMode(MANUAL); // Change Controller to Manual MODE
//
// Blink both lights at together (3 times) to indicate manual mode
//
blinkBothLights(3,250,1,0,1);
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
return(myController.strutMode());
break;
}
break;
//
// Changing Controller LED Settings
//
case 3:
motorsOff();
pushCount = 0;
previousMode = myController.mode();
myController.setMode(SETLED); // Change Controller to change LED MODE
//
// Blink both lights at together (3 times) to indicate LED mode
//
blinkBothLights(3,500,1,0,1);
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
break;
//
// Set default startup Mode
//
case 4:
motorsOff();
pushCount = 0;
previousMode = myController.mode();
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
//
// Blink both lights at together (4 times) to indicate saving startup mode
//
blinkBothLights(4,500,1,0,1);
myController.setMode(SETSTARTUP); // Change Controller to change LED MODE
break;
//
// Changing Controller to Read Diagnostics Mode
//
case 5:
motorsOff();
myController.setMode(READDIAG);
pushCount = 0;
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
//
// Blink both lights at together (5 times) to indicate read diag mode
//
blinkBothLights(5,500,1,0,1);
delay(500);
return(READDIAG);
break;
//
// Changing Controller to RUN Diagnostics Mode
//
case 6:
motorsOff();
myController.setMode(DIAG);
pushCount = 0;
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
//
// Blink both lights at together (6 times) to indicate saving startup mode
//
blinkBothLights(6,500,1,0,1);
delay(500);
return(DIAG);
break;
//
// Changing Controller to Set Thresholds Mode
//
case 7:
motorsOff();
pushCount = 0;
previousMode = myController.mode();
myController.setMode(SETTHRESHOLD); // Change Controller to change LED MODE
//
// Blink both lights at together (7 times) to indicate Set Threshold mode
//
blinkBothLights(7,500,1,0,1);
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
return(SETTHRESHOLD);
break;
//
// Toggle Controller in out of logging mode
//
case 8:
//
// Blink both lights at together (9 times) to indicate logging mode on or Off
//
pushCount = 0;
blinkBothLights(8,500,1,0,1);
if(logMode == 0) {
logMode =1;
logSequence++;
Serial.println(F("**********************************************************************"));
Serial.print(F("LogSequence #:"));
Serial.println(logSequence);
Serial.println(F("******** Strut GForce Transition Thresholds ********"));
Serial.print(F("Medium Threshold: "));
Serial.println( mediumThreshold);
Serial.print(F("Hard Threshold: "));
Serial.println( hardThreshold );
readPrintEpromValues();
Serial.print(F("1:Time, 2:Controller Mode, 3:Controller Target , 4:AccFWD, 5:AccLeftRight,6:AccUP,"));
Serial.print(F("7:Strut Name, 8:FL Strut STatus, 9:FL Target, 10:FL Strut Actual,"));
Serial.print(F("11:Strut Name, 12:FR Strut STatus, 13:FR Target, 14:FR Strut Actual,"));
Serial.print(F("15:Strut Name, 16:RL Strut STatus, 17:RL Target, 18:RL Strut Actual"));
Serial.println(F("19:Strut Name, 20:RR Strut STatus, 21:RR Target, 22:RR Strut Actual "));
}
else if (logMode ==1) logMode =0;
break;
//
// Changing Controller to Calibrate Accelerometer Mode
//
case 9:
motorsOff();
pushCount = 0;
previousMode = myController.mode();
myController.setMode(CALIBRATEACCEL); // Change Controller to
//
// Blink both lights at together (8 times) to indicate Calibration mode
//
blinkBothLights(9,500,1,0,1);
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
return(CALIBRATEACCEL);
break;
}
}
}
//*******************************************************************************************
//
// Read Diagnostic routine
//
//
//*******************************************************************************************
void readDiagnostics(){
while(readControllerSwitch()== NOCHANGE){
// Turn Off All LEDS
offLED(FL_LED);
offLED(FR_LED);
offLED(RL_LED);
offLED(RR_LED);
for(int strut = 0; strut < 4; strut++){
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
//SET strut LED 1->4 to White to indicate testing Front Left Strut
setLED(strut,D_WHITE);
delay(1000);
// Blink Sport Light number of times based on strut nuber as group 4 times To indicate strut
blinkLights(SportLight, (strut+1),300,4,1000);
// See if abort desired
if(readControllerSwitch()!= NOCHANGE) return;
// See if abort desired
if(readControllerSwitch()!= NOCHANGE) return;
// Display Final test results for this Strut
if (myStruts[strut].errorStatus() == NO_FAILURES){
setLED(strut,D_GREEN);
setLights(TourLight,HIGH);
setLights(SportLight,HIGH);
delay(2500);
}else if (myStruts[strut].errorStatus() == MOTOR_COMMAND_FAILURE){
blinkBothLights(3,300,3,1000,0);
setLED(strut,D_RED);
}else if (myStruts[strut].errorStatus() == SIGNAL_WIRE_FAILURE){
setLED(strut,D_YELLOW);
for(int i=0;i<8;i++){
setLights(SportLight,HIGH);
setLights(TourLight,LOW);
delay(250);
setLights(SportLight,LOW);
setLights(TourLight,HIGH);
delay(250);
}
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
}
// See if abort desired
if(readControllerSwitch()!= NOCHANGE) return;
delay(2000);
}
}
}
//*******************************************************************************************
//
// Diagnostic routine
//
//
//*******************************************************************************************
void runDiagnostics(){
while(readControllerSwitch()== NOCHANGE){
// Turn Off All LEDS
offLED(FL_LED);
offLED(FR_LED);
offLED(RL_LED);
offLED(RR_LED);
for(int strut = 0; strut < 4; strut++){
// Turn off Lights
setLights(SportLight,LOW);
setLights(TourLight,LOW);
//SET strut LED 1->4 to White to indicate testing Front Left Strut
setLED(strut,D_WHITE);
delay(1000);
// Blink Sport Light number of times based on strut nuber as group 4 times To indicate strut
blinkLights(SportLight, (strut+1),300,4,1000);
// See if abort desired
if(readControllerSwitch()!= NOCHANGE) return;
//Attempt to set Strut HARD MODE
setStrutDirectMode( strut, HARD, CLEAR);
delay(1000);
// See if abort desired
if(readControllerSwitch()!= NOCHANGE) return;
//Attempt to set Front Left Strut Medium MODE
setStrutDirectMode( strut, MEDIUM, DONT_CLEAR);
delay(1000);
// See if abort desired
if(readControllerSwitch()!= NOCHANGE) return;
//Attempt to set Front Left Strut Soft MODE
setStrutDirectMode( strut, SOFT, DONT_CLEAR);
delay(1000);
// See if abort desired
if(readControllerSwitch()!= NOCHANGE) return;
// Display Final test results for this Strut
if (myStruts[strut].errorStatus() == NO_FAILURES){
setLED(strut,D_GREEN);
setLights(TourLight,HIGH);
setLights(SportLight,HIGH);
delay(2500);
}else{
blinkBothLights(3,300,3,1000,0);
setLED(FL_LED,D_RED);
}
// See if abort desired
if(readControllerSwitch()!= NOCHANGE) return;
delay(2000);
}
}
}
/*************************************************************
*
*
* Routine to read int from EProm
*
*
*************************************************************/
int eepromReadInt(int address){
int value = 0x0000;
value = value | (EEPROM.read(address) << 8);
value = value | EEPROM.read(address+1);
return value;
}
/*************************************************************
*
*
* Routine to Write int to EProm
*
*
*************************************************************/
void eepromWriteInt(int address, int value){
EEPROM.write(address, (value >> 8) & 0xFF );
EEPROM.write(address+1, value & 0xFF);
}
//******************************************************************************************
//
//LSM303DLHC I2C Device Routines
//
//******************************************************************************************
// Send register address and the byte value you want to write the accelerometer and
// loads the destination register with the value you send
void WriteDevRegister(int devAddress, byte data, int regaddress) {
Wire.beginTransmission(devAddress); // device Address
Wire.write(regaddress);
Wire.write(data);
Wire.endTransmission();
}
//
//Send register address to this function and it returns byte value
//for the accelerometer register's contents
byte ReadDevRegister(int devAddress, int regaddress) {
byte data;
Wire.beginTransmission(devAddress); // device Address
Wire.write(regaddress);
Wire.endTransmission();
delayMicroseconds(100);
Wire.requestFrom(devAddress,1); // device Address
data = Wire.read();
Wire.endTransmission();
delayMicroseconds(100);
return data;
}
//**************************************************************************
//
// Accelerometer Initialization Routine
//
//**************************************************************************
void initAccelerometer(void) {
WriteDevRegister(LSM303_ADDRESS_ACC,0x67,0x20); // Enable accelerometer, 200Hz data output
WriteDevRegister(LSM303_ADDRESS_MAG,0x9c,0x00); // Enable temperature sensor, 220Hz data output
WriteDevRegister(LSM303_ADDRESS_MAG,0x20,0x01); // set gain to +/-1.3Gauss
WriteDevRegister(LSM303_ADDRESS_MAG,0x00,0x02); // Enable magnetometer constant conversions
}
//**************************************************************************
//
// Read the X,Y,Z axis values from the accelerometer
//
//**************************************************************************
void readAccelerometerValues() {
float Acc[3];
//
// ForwardAxis axis
// ForwardSign +Acc = Acceleration
// LeftRightAxis axis
// LeftRightSign +Acc = Left Turn
// UpDownAxis axis
// UpDownSign +Acc = Up
//
// reinitialize globals to prepare for new averages
//
ForwardAcceleration = 0.0;
LeftRightAcceleration = 0.0;
UpDownAcceleration = 0.0;
//
// Shift all the previous readings down in the array & Start summing the existing valid entries for the average calculation
//
for ( int i = accelDataPoints -1; i > 0 ; i--){
ForwardAccelerationArray[i] = ForwardAccelerationArray[i-1];
ForwardAcceleration = ForwardAcceleration + ForwardAccelerationArray[i];
LeftRightAccelerationArray[i] = LeftRightAccelerationArray[i-1];
LeftRightAcceleration = LeftRightAcceleration + LeftRightAccelerationArray[i];
UpDownAccelerationArray[i] = UpDownAccelerationArray[i-1];
UpDownAcceleration = UpDownAcceleration + UpDownAccelerationArray[i];
}
//
// get the raw Accelerometer current values off the I2C device
//
getAccelerometer(Acc);
//
// Alias and Assign the current reading to the correct mappings in the normilized array [0]
//
ForwardAccelerationArray[0] = float (ForwardSign * Acc[ForwardAxis]);
LeftRightAccelerationArray[0] = float (LeftRightSign * Acc[LeftRightAxis]);
UpDownAccelerationArray[0] = float (UpDownSign * Acc[UpDownAxis]);
//
// add the current acceleration reading to the global value and finish average calculation
//
ForwardAcceleration = (ForwardAcceleration + ForwardAccelerationArray[0]) / accelDataPoints;
LeftRightAcceleration = (LeftRightAcceleration + LeftRightAccelerationArray[0]) / accelDataPoints;
UpDownAcceleration = (UpDownAcceleration + UpDownAccelerationArray[0]) / accelDataPoints;
if(logMode) {
Serial.print(ForwardAcceleration);
Serial.print(F(","));
Serial.print(LeftRightAcceleration);
Serial.print(F(","));
Serial.print(UpDownAcceleration);
Serial.print(F(","));
}
}
// ******************************************************************************k
//Readsthe X,Y,Z axis values from the accelerometer and sends the values to the
// ******************************************************************************k
void getAccelerometer(float *Acc) {
// accelerometer values
byte xh = ReadDevRegister(LSM303_ADDRESS_ACC,0x29);
byte xl = ReadDevRegister(LSM303_ADDRESS_ACC,0x28);
byte yh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2B);
byte yl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2A);
byte zh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2D);
byte zl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2C);
// need to convert the register contents into a righ-justified 16 bit value
Acc[0] = (xh<<8|xl);
Acc[1] = (yh<<8|yl);
Acc[2] = (zh<<8|zl);
//
// Convert raw Accelerometer readings to g's using calibration values
//
if( Acc[0] >= 0) Acc[0] = float( Acc[0] / XAxisPositive1gCalibration) ;
else Acc[0] = float(Acc[0] / XAxisNegative1gCalibration );
if( Acc[1] >= 0) Acc[1] = float( Acc[1] / YAxisPositive1gCalibration) ;
else Acc[1] = float(Acc[1] / YAxisNegative1gCalibration );
if( Acc[2] >= 0) Acc[2] = float( Acc[2] / ZAxisPositive1gCalibration) ;
else Acc[2] = float(Acc[2] / ZAxisNegative1gCalibration );
}
// ******************************************************************************k
//Readsthe X,Y,Z axis values from the accelerometer and sends the values to the
// ******************************************************************************k
void getRawAccelerometer(float *Acc) {
// accelerometer values
byte xh = ReadDevRegister(LSM303_ADDRESS_ACC,0x29);
byte xl = ReadDevRegister(LSM303_ADDRESS_ACC,0x28);
byte yh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2B);
byte yl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2A);
byte zh = ReadDevRegister(LSM303_ADDRESS_ACC,0x2D);
byte zl = ReadDevRegister(LSM303_ADDRESS_ACC,0x2C);
// need to convert the register contents into a righ-justified 16 bit value
Acc[0] = (xh<<8|xl);
Acc[1] = (yh<<8|yl);
Acc[2] = (zh<<8|zl);
}
//******************************************************************
//Read & Print Eprom Calibration Values
//******************************************************************
void readPrintEpromValues(){
//
// Now Read Back all the Values
//
XAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_X_NEG_1G);
YAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Y_NEG_1G);
ZAxisNegative1gCalibration= eepromReadInt(EPROM_ACC_Z_NEG_1G);
XAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_X_POS_1G);
YAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Y_POS_1G);
ZAxisPositive1gCalibration= eepromReadInt(EPROM_ACC_Z_POS_1G);
Serial.println("**************** Accelerometer Calibration Values *********************");
Serial.print(F("EProm X Neg Axis 1g Calibration: "));
Serial.println(XAxisNegative1gCalibration);
Serial.print(F("EProm Y Neg Axis 1g Calibration: "));
Serial.println(YAxisNegative1gCalibration);
Serial.print(F("EProm Z Neg Axis 1g Calibration: "));
Serial.println(ZAxisNegative1gCalibration);
Serial.print(F("EProm X Pos Axis 1g Calibration: "));
Serial.println(XAxisPositive1gCalibration);
Serial.print(F("EProm Y Pos Axis 1g Calibration: "));
Serial.println(YAxisPositive1gCalibration);
Serial.print(F("EProm Z Pos Axis 1g Calibration: "));
Serial.println(ZAxisPositive1gCalibration);
Serial.println("*************************************************************************");
}
//*************************************************************************************
//
//
// calibrateAccelerometer()
//
//
//************************************************************************************
void calibrateAccelerometer(){
float Acc[3];
float prevAcc[3];
int XAxis;
int YAxis;
int ZAxis;
int button;
//
// Min Max accumulation variables
//
int XAxis_min = 0;
int XAxis_max = 0;
int YAxis_min = 0;
int YAxis_max = 0;
int ZAxis_min = 0;
int ZAxis_max = 0;
//
// Initialize Min Max accumulation variables
//
XAxis_min = 0;
XAxis_max = 0;
YAxis_min = 0;
YAxis_max = 0;
ZAxis_min = 0;
ZAxis_max = 0;
/*
* Intialize to Zero
*/
for(int i=0; i<3 ; i++){
Acc[i] = 0;
}
// Turn off Lights while setting LEDs
setLights(SportLight,LOW);
setLights(TourLight,LOW);
Serial.println(F("********************* Calibration Routine ****************************"));
Serial.println(" o Press Once to see EEprom Calibration & Threshold values ");
Serial.println(" o Press Twice To Exit Calibration Mode ");
Serial.println(" o Press Three Times To Begin Calibration Mode ");
Serial.println(" During Calibration - Press Once to Save Calibration Values & Exit Calibration Routine");
Serial.println(F("**********************************************************************"));
/*
* Stay in this loop until button is pressed multple times to indicate save
*/
while(1){
//read the selector PUSHBUTTON or Rotary and see if they have it in DIAGMODE
button = readPushButtonSwitch();
switch(button){
case NOCHANGE:
break;
case 1:
//
// Ok they want to print current calibration & threshold values
//
//
Serial.println(F("************************ Thresholds **********************************"));
Serial.print(F("Medium Threshold: "));
Serial.println( mediumThreshold);
Serial.print(F("Hard Threshold: "));
Serial.println( hardThreshold );
Serial.println(F("**********************************************************************"));
readPrintEpromValues();
Serial.println(" o Press Once to see EEprom Calibration values ");
Serial.println(" o Press Twice To Exit Calibration Mode ");
Serial.println(" o Press Three Times To Begin Calibration Mode ");
Serial.println(" During Calibration - Press Once to Save Calibration Values & Exit Calibration Routine");
break;
case 2:
//
// Ok they want to Exit
//
Serial.println(F("********************* Exiting Calibration Routine ****************************"));
Serial.println(F("********************* NO Save Performed ****************************"));
//
// Blink both lights at together (3 times) to indicate exiting calibration
//
blinkBothLights(3,500,1,0,1);
// Restore the previous state of the controller
//
myController.setMode(previousMode);
return;
break;
case 3:
//
// Ok they want to Calibrate Accelerometer
//
while(1){
button = readPushButtonSwitch();
if (button != NOCHANGE) break;
for(int i=0; i<3 ; i++){
prevAcc[i] = Acc[i];
}
delay(100);
getRawAccelerometer(Acc);
XAxis =Acc[0];
YAxis =Acc[1];
ZAxis =Acc[2];
//
// Check to make sure previous value = current value
// to weed out noise from acc.
//
if(prevAcc[0] == Acc[0]){
if(Acc[0] < XAxis_min) XAxis_min=XAxis;
if(Acc[0] > XAxis_max) XAxis_max=XAxis;
}
if(prevAcc[1] == Acc[1]){
if(Acc[1] < YAxis_min) YAxis_min=YAxis;
if(Acc[1] > YAxis_max) YAxis_max=YAxis;
}
if(prevAcc[2] == Acc[2]){
if(Acc[2] < ZAxis_min) ZAxis_min=ZAxis;
if(Acc[2] > ZAxis_max) ZAxis_max=ZAxis;
}
/*
Serial.print(" X ");
Serial.print(XAxis );
Serial.print(" Y ");
Serial.print(YAxis );
Serial.print(" Z ");
Serial.println(ZAxis );
*/
Serial.print("MIN X=");
Serial.print(XAxis_min);
Serial.print(" ");
Serial.print("Y=");
Serial.print(YAxis_min);
Serial.print(" ");
Serial.print("Z=");
Serial.print(ZAxis_min);
Serial.print(" MAX X=");
Serial.print(XAxis_max);
Serial.print(" ");
Serial.print("Y=");
Serial.print(YAxis_max);
Serial.print(" ");
Serial.print("Z=");
Serial.println(ZAxis_max);
}
//
// ok their done calibrtaing save values
//
XAxis_min = abs(XAxis_min) ;
eepromWriteInt(EPROM_ACC_X_NEG_1G , XAxis_min);
YAxis_min = abs(YAxis_min) ;
eepromWriteInt(EPROM_ACC_Y_NEG_1G , YAxis_min);
ZAxis_min = abs(ZAxis_min) ;
eepromWriteInt(EPROM_ACC_Z_NEG_1G , ZAxis_min);
eepromWriteInt(EPROM_ACC_X_POS_1G , XAxis_max);
eepromWriteInt(EPROM_ACC_Y_POS_1G , YAxis_max);
eepromWriteInt(EPROM_ACC_Z_POS_1G , ZAxis_max);
//
// Now Read Back & Print all the Values
//
Serial.println(F("************************** Thresholds *****************************"));
Serial.print(F("Medium Threshold: "));
Serial.println( mediumThreshold);
Serial.print(F("Hard Threshold: "));
Serial.println( hardThreshold );
Serial.println(F("**********************************************************************"));
readPrintEpromValues();
Serial.println(F("New Threshold Values have been saved - exiting calibration routine"));
//
// Blink both lights at together (8 times) to indicate saving calibrations & exiting.
//
blinkBothLights(8,500,1,0,1);
// Restore the previous state of the controller
//
myController.setMode(previousMode);
return;
break;
}
}
}
