// Arudino code for Sprinter Wheel Sensor frequency conversion
// Serial output constants
const bool SERIAL_OUTPUT = true; // set to true to send data out serial line
const bool SERIAL_DIAGNOSE = false; // set to true to send back what was recieved
const long SERIAL_REPORT = 1e6; // report over serial every second
// Conversion constants
const bool ENABLE_CONVERSION = false; // Set to true to turn on conversion
const float freq_to_mph = 30.0 * 3.1416 / 55 / 12 / 5280 * 60 * 60; // Hz to mph conversion; assumes 30in wheel diameter, 55 ticks per revolution
const float freq_to_rpm = 1.0 / 55 * 60; // Hz to rpm conversion; assumes 55 ticks per revolution
const float stall_time = 1e6; // If wheel is stopped for 1 second, force speed to 0
float time_conversion = 55.0 / 44; // converts from real sensor with 55 ticks to 44 ticks
// Input/Output pins
const int INPUT_PIN_L = 2; // left wheel input
const int INPUT_PIN_R = 3; // right wheel input
const int OUTPUT_PIN_L = 4; // left wheel output
const int OUTPUT_PIN_R = 5; // right wheel output
// Time variables
unsigned long start_time_left = micros(); // For output cycle
unsigned long start_time_right = micros();
unsigned long last_time_left = micros(); // For input cycle
unsigned long last_time_right = micros();
unsigned long serial_time = micros();
unsigned long current_time = micros();
// Main variables
float t_out_left = 1e6; // period, in microseconds
float t_out_right = 1e6;
float t_in_left = 1e6; // period, in microseconds
float t_in_right = 1e6;
bool high_left = false;
bool high_right = false;
float avg_speed = 0;
// Initial set-up code
void setup() {
// Setup Pin Modes
pinMode(INPUT_PIN_L, INPUT_PULLUP);
pinMode(INPUT_PIN_R, INPUT_PULLUP);
pinMode(OUTPUT_PIN_L, OUTPUT);
pinMode(OUTPUT_PIN_R, OUTPUT);
// Setup Interrupts
//attachInterrupt(INPUT_PIN_L, ISR_L, RISING);
//attachInterrupt(INPUT_PIN_R, ISR_R, RISING);
attachInterrupt(digitalPinToInterrupt(INPUT_PIN_L), ISR_L, RISING);
attachInterrupt(digitalPinToInterrupt(INPUT_PIN_R), ISR_R, RISING);
// Begin Serial
Serial.begin(115200);
// Setup variables
if (! ENABLE_CONVERSION) time_conversion = 1;
start_time_left = micros();
start_time_right = micros();
serial_time = micros();
if (SERIAL_OUTPUT) {
if (ENABLE_CONVERSION) Serial.println("Speed Conversion is ON");
Serial.print("Time (ms)"); // Overall Time
Serial.print("\t");
Serial.print("Left Wheel Input (rpm)"); // Speed from left wheel sensor
Serial.print("\t");
Serial.print("Right Wheel Input (rpm)"); // Speed from right wheel sensor
Serial.print("\t");
Serial.print("Left Wheel Output (mph)"); // Speed output to car
Serial.print("\t");
Serial.println("Right Wheel Output (mph)"); // Speed output to car
}
}
void loop() {
// update times
current_time = micros();
if (current_time - start_time_left > t_out_left) {
t_out_left = t_in_left * time_conversion;
if (current_time - last_time_left > stall_time) t_out_left = 1e9;
start_time_left = current_time;
}
if (current_time - start_time_right > t_out_right) {
t_out_right = t_in_right * time_conversion;
if (current_time - last_time_right > stall_time) t_out_right = 1e9;
start_time_right = current_time;
}
// write output waveform
if (current_time - start_time_left < t_out_left/2) { // write HIGH during 1st half of cycle
if (! high_left) {
digitalWrite(OUTPUT_PIN_L, HIGH);
high_left = true;
}
}
else { // write LOW
if (high_left) {
digitalWrite(OUTPUT_PIN_L, LOW);
high_left = false;
}
}
if (current_time - start_time_right < t_out_right/2) { // write HIGH during 1st half of cycle
if (! high_right) {
digitalWrite(OUTPUT_PIN_R, HIGH);
high_right = true;
}
}
else { // write LOW
if (high_right) {
digitalWrite(OUTPUT_PIN_R, LOW);
high_right = false;
}
}
// write to serial
if (SERIAL_OUTPUT && current_time - serial_time > SERIAL_REPORT) {
Serial.print(millis()); // Overall Time
Serial.print("\t");
Serial.print(1e6 / t_in_left * freq_to_rpm); // Speed from left wheel sensor
Serial.print("\t");
Serial.print(1e6 / t_in_right * freq_to_rpm); // Speed from right wheel sensor
Serial.print("\t");
Serial.print(1e6 / t_out_left * freq_to_mph); // Speed output to car
Serial.print("\t");
Serial.println(1e6 / t_out_right * freq_to_mph); // Speed output to car
serial_time = current_time;
}
}
// end loop
// Interrupt Routines - runs once on each input cycle (when encoder rises)
void ISR_L() {
// Update times
current_time = micros();
t_in_left = current_time - last_time_left;
last_time_left = current_time;
}
void ISR_R() {
// Update times
current_time = micros();
t_in_right = current_time - last_time_right;
last_time_right = current_time;
}