/* FreqMeasureMulti - Servo
* http://pjrc.com/teensy/td_libs_FreqMeasure.html
* http://pjrc.com/teensy/td_libs_Servo.html
*
* Board FreqMeasureMulti Supported Pins on FTM0
* ----- --------------
* Teensy LC 6, 9, 10, 20, 22, 23
* Teensy 3.1 5, 6, 9, 10, 20, 21, 22, 23
* Teensy 3.2 5, 6, 9, 10, 20, 21, 22, 23
*
*/
#include <FreqMeasureMulti.h>
#include <Servo.h>
#include <EEPROM.h>
#define debug
#define CHANNELS 4
#define NEUTRAL 1500
#define REVERSE_LIMIT 1200
#define FORWARD1_LIMIT 1800
#define FORWARD2_LIMIT 2000
#define MAX_FRAME_INTERVAL 22000
#define NEW_EEPROM_COUNT_THRESHOLD 100
#define RESERVED_EEPROM_ADDR 0
#define MIN_THROTTLE_ADDR sizeof(uint16_t)
#define MAX_THROTTLE_ADDR sizeof(uint16_t)*2
elapsedMicros sinceFrame;
enum states {NOT_RC, RC_STOPPED, RC_DRIVING, STOPPED, REVERSE, FORWARD1, FORWARD2};
states state;
// Measure width of 4 individual Futaba servo channels(R2004GF)
// channels 1 and 2 begin simultaneously, and channel 3 and 4 begin simultaneously a few ns after start of 1 and 2.
FreqMeasureMulti channel1;
FreqMeasureMulti channel2;
FreqMeasureMulti channel3;
FreqMeasureMulti channel4;
// store the updated value in ch[x], for now, put default values there.
uint16_t ch_microseconds[CHANNELS +1] = {0,NEUTRAL,NEUTRAL,NEUTRAL,NEUTRAL};
// and output 4 regular servo channels
Servo servo1; // create servo objects
Servo servo2;
Servo servo3; // create servo objects
Servo servo4;
static const uint8_t throttlePedalPin = 14;
uint16_t throttlePedalValue = 0;
uint16_t minThrottlePedalValue = 400;
uint16_t maxThrottlePedalValue = 600;
static const uint8_t blueShifterPin= 2;
static const uint8_t blackShifterPin= 3;
uint16_t leftMCvalue = NEUTRAL;
uint16_t rightMCvalue = NEUTRAL;
void setup()
{
#ifdef debug
Serial.begin(57600);
while (!Serial) ; // wait for Arduino Serial Monitor
delay(10);
Serial.println("FreqMeasureMulti and Servo Begin");
delay(10);
#endif
channel1.begin(9,FREQMEASUREMULTI_MARK_ONLY); // purple wire
channel2.begin(10,FREQMEASUREMULTI_MARK_ONLY); // blue wire
channel3.begin(20,FREQMEASUREMULTI_MARK_ONLY); // green wire
channel4.begin(22,FREQMEASUREMULTI_MARK_ONLY); // yellow wire
servo1.attach(4); // white wire
servo2.attach(5); // grey wire
servo3.attach(6); // brown wire
servo4.attach(7); // orange wire
pinMode(throttlePedalPin, INPUT);
pinMode(blueShifterPin, INPUT_PULLUP);
pinMode(blackShifterPin, INPUT_PULLUP);
pinMode(13, OUTPUT); // LED
uint16_t temp;
EEPROM.get(MIN_THROTTLE_ADDR, temp);
if(0 == temp)
{EEPROM.put(MIN_THROTTLE_ADDR, minThrottlePedalValue);} // first time only
EEPROM.get(MAX_THROTTLE_ADDR, temp);
if(0 == temp)
{EEPROM.put(MAX_THROTTLE_ADDR, maxThrottlePedalValue);} // first time only
// every time
EEPROM.get(MIN_THROTTLE_ADDR, minThrottlePedalValue);
EEPROM.get(MAX_THROTTLE_ADDR, maxThrottlePedalValue);
sinceFrame = 0;
}
void loop()
{ // min and max of throttle are checked
static uint16_t tempMinThrottlePedalValue = minThrottlePedalValue;
static uint16_t tempMaxThrottlePedalValue = maxThrottlePedalValue;
static uint16_t minCount = 0;
static uint16_t maxCount = 0;
if (channel1.available() && channel2.available() && channel3.available() ) // valid RC frame
{
// read RC controls
sinceFrame = 0;
ch_microseconds[1] = FreqMeasureMulti::countToMicroseconds(channel1.readLatest());
ch_microseconds[2] = FreqMeasureMulti::countToMicroseconds(channel2.readLatest());
ch_microseconds[3] = FreqMeasureMulti::countToMicroseconds(channel3.readLatest());
ch_microseconds[4] = FreqMeasureMulti::countToMicroseconds(channel4.readLatest()); // don't care if ch 4 is valid, but we'll take it anyway
#ifdef debug
Serial.print("in RC loop. sinceFrame ");
Serial.println(sinceFrame);
Serial.print(ch_microseconds[1]);
Serial.print(", ");
Serial.print(ch_microseconds[2]);
Serial.print(", ");
Serial.print(ch_microseconds[3]);
Serial.print(", ");
Serial.println(ch_microseconds[4]);
Serial.println();
#endif
if(ch_microseconds[3] > NEUTRAL + 200)// left stick vertical, ratchet, non-spring-return, down
{// RC signal received, and override selected
ch_microseconds[0] = 1;// boolean indicator for RC control
if(((ch_microseconds[1] > NEUTRAL-60)&&(ch_microseconds[1] < NEUTRAL+60))&&(((ch_microseconds[2] > NEUTRAL-60)&&(ch_microseconds[2] < NEUTRAL+60))))
{// right stick within deadband
state = RC_STOPPED;
leftMCvalue = NEUTRAL;
rightMCvalue = NEUTRAL;
#ifdef debug
Serial.print("state RC_STOPPED. sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.println();
#endif
}
else
{// RC_1pulse = steer- increases to right, RC_2pulse = speed- increases to up
state = RC_DRIVING;
leftMCvalue = ch_microseconds[2]+(ch_microseconds[1]-(NEUTRAL+20));
rightMCvalue = ch_microseconds[2]-(ch_microseconds[1]-(NEUTRAL+20));
#ifdef debug
Serial.print("state RC_DRIVING. sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.println();
#endif
}
servo1.writeMicroseconds(leftMCvalue);
servo2.writeMicroseconds(rightMCvalue);
servo3.writeMicroseconds(ch_microseconds[3]);// unused as output, push DOWN to override, UP to hand over local control
servo4.writeMicroseconds(ch_microseconds[4]);// might be used as pass through, but just as easy to wire directly from receiver to servo
#ifdef debug
Serial.print("writing to ESC RC. sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.print("ch_microseconds[3] ");
Serial.println(ch_microseconds[3]);
Serial.print("ch_microseconds[4] ");
Serial.println(ch_microseconds[4]);
Serial.println();
#endif
}
else
{
ch_microseconds[0] = 0;
state = NOT_RC;
#ifdef debug
Serial.print("state NOT_RC. sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.println();
#endif
}
}
if(sinceFrame > MAX_FRAME_INTERVAL || state == NOT_RC)// RC equipment off, out of range, one or more critical channels disconnected, or ch3 stick is down
{
// read local controls
sinceFrame = 0;
ch_microseconds[0] = 0;// can be used as a boolean flag to determine if under RC control
#ifdef debug
Serial.print("in local control loop. sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.println();
#endif
if((throttlePedalValue = analogRead(throttlePedalPin)) < (minThrottlePedalValue + 40))
{ // no throttle applied
state = STOPPED;
leftMCvalue = NEUTRAL;
rightMCvalue = NEUTRAL;
#ifdef debug
Serial.print("state STOPPED, throttle ");
Serial.print(throttlePedalValue);
Serial.print(" sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.println();
#endif
}
else if(digitalRead(blueShifterPin) ) //blue high note: If shifter pins are disconnected, they will both be pulled high and state will default to REVERSE
{ // low top speed reverse
state = REVERSE;
leftMCvalue = map(throttlePedalValue, minThrottlePedalValue, maxThrottlePedalValue, NEUTRAL, REVERSE_LIMIT);
rightMCvalue = map(throttlePedalValue, minThrottlePedalValue, maxThrottlePedalValue, NEUTRAL, REVERSE_LIMIT);
#ifdef debug
Serial.print("state REVERSE, throttle ");
Serial.print(throttlePedalValue);
Serial.print(" sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.println();
#endif
}
else if(digitalRead(blackShifterPin) && !(digitalRead(blueShifterPin))) //black high
{ // low top speed forward
state = FORWARD1;
leftMCvalue = map(throttlePedalValue, minThrottlePedalValue, maxThrottlePedalValue, NEUTRAL, FORWARD1_LIMIT);
rightMCvalue = map(throttlePedalValue, minThrottlePedalValue, maxThrottlePedalValue, NEUTRAL, FORWARD1_LIMIT);
#ifdef debug
Serial.print("state FORWARD1, throttle ");
Serial.print(throttlePedalValue);
Serial.print(" sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.println();
#endif
}
else
{ // high top speed forward
state = FORWARD2;
leftMCvalue = map(throttlePedalValue, minThrottlePedalValue, maxThrottlePedalValue, NEUTRAL, FORWARD2_LIMIT);
rightMCvalue = map(throttlePedalValue, minThrottlePedalValue, maxThrottlePedalValue, NEUTRAL, FORWARD2_LIMIT);
#ifdef debug
Serial.print("state FORWARD2, throttle ");
Serial.print(throttlePedalValue);
Serial.print(" sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.println();
#endif
}
servo1.writeMicroseconds(leftMCvalue);
servo2.writeMicroseconds(rightMCvalue);
servo3.writeMicroseconds(ch_microseconds[3]);// unused, just push up to override, down to handover local control
servo4.writeMicroseconds(ch_microseconds[4]);// might be used as pass through, but just as easy to wire directly from receiver to servo
static uint16_t tempMinThrottlePedalValue = minThrottlePedalValue;
static uint16_t tempMaxThrottlePedalValue = maxThrottlePedalValue;
static uint16_t minCount = 0;
static uint16_t maxCount = 0;
if(throttlePedalValue <= minThrottlePedalValue)// flirting with a new min
{
if(throttlePedalValue == tempMinThrottlePedalValue)// not the first time at this value, make sure it isn't a transient
{
minCount++;
if(minCount > NEW_EEPROM_COUNT_THRESHOLD)// we have a new bottom
{
minThrottlePedalValue = tempMinThrottlePedalValue;// this affects scaling of throttle and may change the feel of throttle response
EEPROM.put(MIN_THROTTLE_ADDR, minThrottlePedalValue);
#ifdef debug
Serial.print("new min throttle ");
Serial.println(minThrottlePedalValue);
#endif
}
}
else // new temp minimum
{
tempMinThrottlePedalValue = throttlePedalValue; // new candidate for bottom. Perhaps the potentiometer mount shifted.
minCount = 0;
}
}
if(throttlePedalValue >= maxThrottlePedalValue)// flirting with a new max
{
if(throttlePedalValue == tempMaxThrottlePedalValue)// not the first time at this value, make sure it isn't a transient
{
maxCount++;
if(maxCount > NEW_EEPROM_COUNT_THRESHOLD)// we have a new top
{
maxThrottlePedalValue = tempMaxThrottlePedalValue;// this affects scaling of throttle and may change the feel of throttle response
EEPROM.put(MAX_THROTTLE_ADDR, maxThrottlePedalValue);
#ifdef debug
Serial.print("new max throttle ");
Serial.println(maxThrottlePedalValue);
#endif
}
}
else // new temp maximum
{
tempMaxThrottlePedalValue = throttlePedalValue; // new candidate for top. Perhaps the potentiometer mount shifted.
maxCount = 0;
}
}
#ifdef debug
Serial.print("writing to ESC local. sinceFrame ");
Serial.println(sinceFrame);
Serial.print("leftMCvalue ");
Serial.print(leftMCvalue);
Serial.print(" rightMCvalue ");
Serial.println(rightMCvalue);
Serial.print("ch_microseconds[3] ");
Serial.println(ch_microseconds[3]);
Serial.print("ch_microseconds[4] ");
Serial.println(ch_microseconds[4]);
Serial.println();
#endif
}// RC receiver has LED to indicate good RC signal(green) or bad(red). The Teensy LED lights (orange) to indicate that the tractor is under Hank's local control.
// if the receiver LED is ever Red or dark, and the Teensy LED is not orange, there is a problem.
// RC receiver green and Teensy LED orange, means that RC can take control at any time by moving stick 3 down.
digitalWriteFast(13, ch_microseconds[0]?LOW:HIGH); // does this line get the fast optimization? The parameter is technically a constant, but a variable must be accessed for the ternary.
//almost same as
//if(ch_microseconds[0]){digitalWriteFast(13, LOW);}
//else {digitalWriteFast(13, HIGH);}
}
