rc_receiver_driving/03_main.ino

char outputStr[100];
int16_t averages[2][AVG_PERIODS];

void loop() {
#ifdef SCAN_MODE
	for(byte ch = LOW_CHANNEL; ch <= NUM_CHANNELS; ch++) {
		int val = channelVals[ch];
		sprintf(outputStr, "Channel-%d:%d\t", ch, val);
		Serial.print(outputStr);
	}
	Serial.println();
#else
	int throttle = getThrottle();
	throttle = restrictThrottlePower(throttle);
	throttle = getAverage(throttle);

	sprintf(outputStr, "Throttle:%d", throttle);
	Serial.println(outputStr);

	digitalWrite(LEFT_MOTOR_DIRECT_PIN, throttle > 0 ? HIGH : LOW);
	analogWrite(LEFT_MOTOR_PIN, throttle > 0 ? throttle : -throttle);
#endif

	delay(SCAN_DELAY);
}

const int throtDeadHigh = THROT_NONE + THROT_DEAD_ZONE,
	throtDeadLow = THROT_NONE - THROT_DEAD_ZONE;

int getThrottle() {
	int val = channelVals[THROT_CHANNEL];
#ifndef GRAPH
	Serial.println();
	Serial.print("Channel value: ");
	Serial.print(val);
	Serial.print("\t");
#endif
	if(val <= THROT_OFF) {
#ifndef GRAPH
		Serial.println("Throttle is off");
#endif
		return 0;
	} else if(val < throtDeadHigh && val > throtDeadLow) {
#ifndef GRAPH
		Serial.println("Throttle in dead zone");
#endif
		return 0;
	}
	

	int throttle = val < THROT_NONE
		? map(val, THROT_MIN, THROT_NONE, -OUTPUT_RANGE, 0)
		: map(val, THROT_NONE, THROT_MAX, 0, OUTPUT_RANGE);
	
	throttle = constrain(throttle, -OUTPUT_RANGE, OUTPUT_RANGE);
#ifndef GRAPH
	Serial.print("Throttle: ");
	Serial.println(throttle);
#endif
	return throttle;
}

int restrictThrottlePower(int throttle) {
	// Reduce the average voltage to something safe for the motor to consume.
	throttle = (throttle * MAX_POWER_PERCENT) / 100;

#ifndef GRAPH
	Serial.print("Restricted throttle:\t");
	Serial.println(throttle);
#endif

	// Use the current value of the knob to reduce the speed.
	int speedKnob = channelVals[SPEED_CHANNEL];
	speedKnob = map(speedKnob, SPEED_MIN, SPEED_MAX, 0, 100);
	speedKnob = constrain(speedKnob, 0, 100);
	throttle = (throttle * speedKnob) / 100;

#ifndef GRAPH
	Serial.print("Speed knob:\t");
	Serial.println(speedKnob);
	Serial.print("Speed adjusted throttle:\t");
	Serial.println(throttle);
#endif
	
	return throttle;
}

int getAverage(const int throttle) {
	int total = throttle;
	for(byte i = 0; i < AVG_PERIODS-1; i++) {
		total += averages[i];
		averages[i] = averages[i+1];
	}
	averages[AVG_PERIODS-1] = throttle;
	total /= AVG_PERIODS;

#ifndef GRAPH
	Serial.print("Average adjusted throttle:\t");
	Serial.println(total);
#endif

	return total;
}