Control two wheels w/steering + add deadman switch

- Adjusted throttle values to be -1000 to +1000 to make maths easier.
- Steering adjusts per-wheel throttles by 50% of throttle (arbitrary amount).
  - Makes sure the max power to a wheel never exceeds the limits.
  - Averages are now per-wheel.
- Deadman called in the loop makes sure everything stops if the controller
  disconnects.
- Had to change logging method to not be generic as the compiler got confused
  with `long long int&` for `Serial.println()`.
- Rearranged constants into better groupings.

02_logging.ino

@@ -12,8 +12,7 @@ void debugln(T val) {
 #endif
 }
 
-template <typename T, typename Y>
-void debugln(T str, Y val) {
+void debugln(const char* str, long val) {
 #ifdef DEBUG
 	Serial.print(str);
 	Serial.println(val);

02_readPins.ino

@@ -1,9 +1,8 @@
 unsigned long highStartTimes[NUM_CHANNELS + 1];
+unsigned long lastInterrupt = 0;
 
 void readPins() {
 	const unsigned long now = micros();
-//	Serial.print("Interrupt started: ");
-//	Serial.println(now);
 	for(byte ch = 1; ch <= NUM_CHANNELS; ch++) {
 		byte pin = channels[ch];
 		boolean isHigh = digitalRead(pin) == HIGH;
@@ -17,4 +16,19 @@ void readPins() {
 			highStartTimes[ch] = 0;
 		}
 	}
+	lastInterrupt = millis();
+}
+
+
+/**
+ * Detect controller disconnects by checking if it's been too long since we last had
+ * an interurt occur. If so, set all channel values to 0.
+ */
+void deadmanSwitch() {
+	if(millis() - lastInterrupt > DEAD_MAN_MILLIS) {
+		debugln(F("Deadman switch activated!"));
+		for(byte ch = 1; ch <= NUM_CHANNELS; ch++) {
+			channelVals[ch] = 0;
+		}
+	}
 }

03_main.ino

@@ -1,11 +1,17 @@
 char outputStr[100];
-int16_t averages[AVG_PERIODS];
+int16_t leftAverages[AVG_PERIODS];
+int16_t rightAverages[AVG_PERIODS];
 
 const int throtDeadHigh = THROT_NONE + THROT_DEAD_ZONE,
 	throtDeadLow = THROT_NONE - THROT_DEAD_ZONE;
 
+const int steerDeadHigh = STEER_NONE + STEER_DEAD_ZONE,
+	steerDeadLow = STEER_NONE - STEER_DEAD_ZONE;
+
+
 void loop() {
 	const long startTime = millis();
+	deadmanSwitch();
 	
 #ifdef SCAN_MODE
 	for(byte ch = LOW_CHANNEL; ch <= NUM_CHANNELS; ch++) {
@@ -18,21 +24,24 @@ void loop() {
 	debugln();
 	debugln("===================");
 
-	int throttle = getThrottle();
-	throttle = restrictThrottlePower(throttle);
-	throttle = getAverage(throttle);
+	int throttle = getThrottle(),
+		steer = getSteering();
 
-	sprintf(outputStr, "Throttle:%d", throttle);
-	Serial.println(outputStr);
+	driveWheel(throttle, steer, LEFT),
+	driveWheel(throttle, steer, RIGHT);
 
-	digitalWrite(LEFT_MOTOR_DIRECT_PIN, throttle > 0 ? HIGH : LOW);
-	analogWrite(LEFT_MOTOR_PIN, throttle > 0 ? throttle : -throttle);
+	Serial.println();
 #endif
 
 	const long now = millis();
-	delay(SCAN_DELAY + (startTime - now));
+	const long sleepTime = SCAN_DELAY + (startTime - now);
+	delay(sleepTime);
 }
 
+
+/**
+ * Return the throttle value between -THROT_RANGE (reverse) and +THROT_RANGE (forward)
+ */
 int getThrottle() {
 	int val = channelVals[THROT_CHANNEL];
 
@@ -46,20 +55,58 @@ int getThrottle() {
 		debugln("Throttle in dead zone");
 		return 0;
 	}
-	
 
+	// Map the value to a value from 0 to THROT_RANGE range (or -THROT_RANGE to 0)
 	int throttle = val < THROT_NONE
-		? map(val, THROT_MIN, THROT_NONE, -OUTPUT_RANGE, 0)
-		: map(val, THROT_NONE, THROT_MAX, 0, OUTPUT_RANGE);
+		? map(val, THROT_MIN, THROT_NONE, -THROT_RANGE, 0)
+		: map(val, THROT_NONE, THROT_MAX, 0, THROT_RANGE);
 	
-	throttle = constrain(throttle, -OUTPUT_RANGE, OUTPUT_RANGE);
+	// Constrain the value in case the receiver outputs a value
+	// outside the expected range.
+	throttle = constrain(throttle, -THROT_RANGE, THROT_RANGE);
 
 	debugln("Throttle: ", throttle);
 
 	return throttle;
 }
 
-int restrictThrottlePower(int throttle) {
+
+/**
+ * Get the value for steering, between -STEER_RANGE (left) and +STEER_RANGE (right).
+ */
+int getSteering() {
+	int val = channelVals[STEER_CHANNEL];
+
+	debug("Steer channel: ");
+	debug(val);
+	debug("\t");
+
+	if(val < steerDeadHigh && val > throtDeadLow) {
+		debugln("Steering in dead zone");
+		return 0;
+	}
+
+	// Map the value to a value from 0 to STEER_RANGE range (or -STEER_RANGE to 0)
+	int steer = val < STEER_NONE
+		? map(val, STEER_LEFT, STEER_NONE, -STEER_RANGE, 0)
+		: map(val, STEER_NONE, STEER_RIGHT, 0, STEER_RANGE);
+
+	// Constrain the value in case the receiver outputs a value
+	// outside the expected range.
+	steer = constrain(steer, -STEER_RANGE, STEER_RANGE);
+
+	debugln("Steer: ", steer);
+
+	return steer;
+}
+
+
+/**
+ * Restrict the motor power based on the speed control dial.
+ * Takes int from 0-255
+ * Returns int 0-255
+ */
+int restrictThrottlePower(long throttle) {
 	// Reduce the average voltage to something safe for the motor to consume.
 	throttle = (throttle * MAX_POWER_PERCENT) / 100;
 
@@ -73,11 +120,21 @@ int restrictThrottlePower(int throttle) {
 
 	debugln("Speed knob:\t", speedKnob);
 	debugln("Speed adjusted throttle:\t", throttle);
+
+	throttle = map(throttle, -THROT_RANGE, THROT_RANGE, -OUTPUT_RANGE, OUTPUT_RANGE);
+	debugln("Range adjusted throttle:\t", throttle);
 	
 	return throttle;
 }
 
-int getAverage(const int throttle) {
+
+/**
+ * Get the average throttle for the last AVG_OVER_MILLIS ms
+ *
+ * Input: value from 0-255
+ * Return: value from 0-255
+ */
+int getAverage(const int throttle, int16_t* averages) {
 	long long total = throttle;
 	for(byte i = 0; i < AVG_PERIODS-1; i++) {
 		total += averages[i];
@@ -90,3 +147,55 @@ int getAverage(const int throttle) {
 
 	return total;
 }
+
+
+/**
+ * Using the current throttle and steering values, determine the needed output
+ * value for a given wheel (LEFT or RIGHT), then drive that motor.
+ */
+int driveWheel(int throttle, const int steer, const byte wheel) {
+	// Negative steer = turn left = left backwards + right forwards
+	// Positive steer = turn right = left forwards + right backwards
+	int16_t* averages;
+	debugln();
+			
+	switch(wheel) {
+		case LEFT:
+			debugln(F("=== LEFT ==="));
+			Serial.print(F("Left:"));
+			throttle += steer;
+			averages = leftAverages;
+			break;
+		case RIGHT:
+			debugln();
+			debugln(F("=== RIGHT ==="));
+			Serial.print(F("Right:"));
+			throttle -= steer;
+			averages = rightAverages;
+			break;
+	}
+
+	debugln("Adjusted throttle:\t", throttle);
+
+	// Make sure steering didn't throw it out of range
+	throttle = constrain(throttle, -THROT_RANGE, THROT_RANGE);
+	debugln("Constrained throttle:\t", throttle);
+
+	// Restrict it to the max power dial
+	throttle = restrictThrottlePower(throttle);
+
+	// Average the power so it doesn't overwhelm the wheel
+	throttle = getAverage(throttle, averages);
+
+	debug("Wheel power: ");
+	Serial.print(throttle);
+	Serial.print(F("\t"));
+
+	digitalWrite(
+			wheel == LEFT ? LEFT_MOTOR_DIRECT_PIN : RIGHT_MOTOR_DIRECT_PIN,
+			throttle > 0 ? HIGH : LOW
+		);
+	analogWrite(
+			wheel == LEFT ? LEFT_MOTOR_PIN : RIGHT_MOTOR_PIN,
+			throttle > 0 ? throttle : -throttle);
+}

rc_receiver_driving.ino

@@ -3,18 +3,31 @@
 //////////////////////////////////////////////////
 
 #define MAX_POWER_PERCENT 90 // The maximum percentage on time, used to decrease average voltage to the motor.
-#define OUTPUT_RANGE 255 // The maximum value for PWM output
-///////////////////////
-
 #define SCAN_DELAY 10 // Delay between updates in ms
-#define AVG_OVER_MILLIS 2000 // Number of milliseconds to average the speed over
-#define AVG_PERIODS AVG_OVER_MILLIS/SCAN_DELAY // Number of periods to average the speed over (reduces random variances)
+#define AVG_OVER_MILLIS 1500 // Number of milliseconds to average the speed over
 
 /////////////////////////////////////////////////////////////////////////
 // Output config
 /////////////////////////////////////////////////////////////////////////
 
 //#define SCAN_MODE // If set, output channel values rather than driving the robot.
+//#define DEBUG // If set, adjust timings and output debug lines
+
+#ifdef DEBUG
+#define SCAN_DELAY 1000
+#define AVG_OVER_MILLIS 3000
+#endif
+
+/////////////////////////////////////////////////////////////////////////
+// Internal values
+/////////////////////////////////////////////////////////////////////////
+
+#define LEFT 0
+#define RIGHT 1
+#define OUTPUT_RANGE 255 // The maximum value for PWM output
+#define THROT_RANGE 1000 // Value output by throttle (-THROT_RANGE to THROT_RANGE), later constrained to OUTPUT_RANGE
+#define STEER_RANGE THROT_RANGE/2 // Value output by steering (-STEER_RANGE to STEER_RANGE) to adjust throttles
+#define AVG_PERIODS AVG_OVER_MILLIS/SCAN_DELAY // Number of periods to average the speed over (reduces random variances)
 
 /////////////////////////////////////////////////////////////////////////
 // Channel pins
@@ -32,6 +45,8 @@ int channelVals[NUM_CHANNELS + 1];
 // Channel values
 /////////////////////////////////////////////////////////////////////////
 
+#define DEAD_MAN_MILLIS 100 // Time to wait after last interrupt before figuring the controller turned off
+
 // Right horizontal (steering)
 #define STEER_CHANNEL 1
 #define STEER_LEFT 1300
@@ -65,7 +80,7 @@ int channelVals[NUM_CHANNELS + 1];
 // Output pins
 /////////////////////////////////////////////////////////////////////////
 
-#define LEFT_MOTOR_PIN 5
 #define LEFT_MOTOR_DIRECT_PIN 4
+#define LEFT_MOTOR_PIN 5
 #define RIGHT_MOTOR_PIN 6
 #define RIGHT_MOTOR_DIRECT_PIN 7