World State

This code displays the current information about the motors and sensors.

#ifndef WORLDSTATE_H
#define WORLDSTATE_H

void initializeLog();
void worldState(bool updateLog, int heading, float power, float motor1ratio, float motor2ratio, float motor3ratio, float motor4ratio);
void closeLog();

#endif // WORLDSTATE_H

// Required FEH libraries
#include <FEHRPS.h>
#include <FEHIO.h>
#include <FEHLCD.h>
#include <FEHUtility.h>
#include <FEHBattery.h>
#include <FEHSD.h>

// Required custom libraries
#include "constants.h"
#include "worldstate.h"

/* This function initializes the SD card for logging. It closes any open logs, opens a new log, and prints a log header.
 * From past experiences, it is best to try to close all previous logs in case any are still open.
 * Last modified: 3/14/2016 JKL
 */
void initializeLog(){
    // Must close any remaining log files left over to prevent any SD card issues
    for (int x = 0; x<=100; x++) {
        LCD.WriteRC("Closing Log:", 7, 3);
        LCD.WriteRC(x, 7, 16);
        SD.CloseLog();
    }
    SD.OpenLog();
    SD.Printf("Title: %s - Version: %f\n", TITLE, VERSION);
    SD.Printf("Course: %c - Red: %d - White: %d - Blue: %d\n", RPS.CurrentRegionLetter(), RPS.RedSwitchDirection(), RPS.WhiteSwitchDirection(), RPS.BlueSwitchDirection() );
    SD.Printf("Time\tRPSTime\tBatVolt\tMS1\tMS2\tMS3\tMS4\tMS5\tMS6\tMS7\tMS8\tCDSCell\tMot1\tMot2\tMot3\tMot4\tH.\tRPS_H\tRPS_X\tRPS_Y");
} // end initializeLog function

/* This function closes the SD log. This allows for the FEHSD.h library to be isolated in this program.
 * Last Modified: 3/14/2016 JKL
 */
void closeLog() {
    SD.CloseLog();
} // end closeLog function

/* This function prints the world state to the Proteus LCD and, if requested, to a log file
 * Inputs: - bool updateLog - true writes the world state to the log file / false does not
 *         - The rest of the arguments are what is written to the log file
 * Last Modified: 3/14/2016 JKL
 */
void worldState(bool updateLog, int heading, float power, float motor1ratio, float motor2ratio, float motor3ratio, float motor4ratio) {
    LCD.Clear(BLACK);

    LCD.DrawRectangle(2*12+1, 4*17+1, 22*12-1, 6*16-1);
    //    LCD.WriteRC("*                    *", 5, 2);
    //    LCD.WriteRC("*                    *", 6, 2);
    //    LCD.WriteRC("*                    *", 7, 2);
    //    LCD.WriteRC("*                    *", 8, 2);

    LCD.SetFontColor(WHITE);

    LCD.WriteRC(TimeNow(), 13, 5);
    LCD.WriteRC(RPS.Time(), 13, 15);

    if (updateLog)
        SD.Printf("\n%f\t%f\t", TimeNow(), RPS.Time());

    if (updateLog)
        SD.Printf("%f\t", Battery.Voltage());

    LCD.WriteRC( (int) microswitch1.Value(), 0, 2);
    LCD.WriteRC( (int) microswitch2.Value(), 0, 23);
    LCD.WriteRC( (int) microswitch3.Value(), 2, 25);
    LCD.WriteRC( (int) microswitch4.Value(), 11, 25);
    LCD.WriteRC( (int) microswitch5.Value(), 13, 23);
    LCD.WriteRC( (int) microswitch6.Value(), 13, 2);
    LCD.WriteRC( (int) microswitch7.Value(), 11, 0);
    LCD.WriteRC( (int) microswitch8.Value(), 2, 0);

    if (updateLog)
        SD.Printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t",
              (int) microswitch1.Value(), (int) microswitch2.Value(), (int) microswitch3.Value(), (int) microswitch4.Value(),
              (int) microswitch5.Value(), (int) microswitch6.Value(), (int) microswitch7.Value(), (int) microswitch8.Value());

    if (0.0 < cdscell.Value() && cdscell.Value() < 0.8)
        LCD.SetFontColor(RED);
    if (1.0 < cdscell.Value() && cdscell.Value() < 1.8)
        LCD.SetFontColor(BLUE);
    LCD.WriteRC( cdscell.Value(), 0, 11);

    if (updateLog)
        SD.Printf("%f\t", cdscell.Value());

    LCD.SetFontColor(WHITE);
    LCD.WriteRC("Mot1", 2, 2);
    LCD.WriteRC(power * motor1ratio, 3, 2);
    LCD.WriteRC("Mot2", 2, 20);
    LCD.WriteRC(power * motor2ratio, 3, 18);
    LCD.WriteRC("Mot3", 11, 20);
    LCD.WriteRC(power * motor3ratio, 10, 18);
    LCD.WriteRC("Mot4", 11, 2);
    LCD.WriteRC(power * motor4ratio, 10, 2);

    if (updateLog)
        SD.Printf("%f\t%f\t%f\t%f\t", power * motor1ratio, power * motor2ratio, power * motor3ratio, power * motor4ratio);

    LCD.WriteRC("H:", 2, 10);
    LCD.WriteRC(heading, 2, 12);

    if (updateLog)
        SD.Printf("%d\t", heading);

    LCD.WriteRC("B:", 11, 9);
    LCD.WriteRC(RPS.Heading(), 11, 11);
    LCD.WriteRC("X:", 12, 4);
    LCD.WriteRC(RPS.X(), 12, 6);
    LCD.WriteRC("Y:", 12, 14);
    LCD.WriteRC(RPS.Y(), 12, 16);

    if (updateLog)
        SD.Printf("%f\t%f\t%f\t", RPS.Heading(), RPS.X(), RPS.Y());
} // end worldState function

Here is a picture that shows how to information was displayed onto the Proteus LCD screen.

WorldState