MCU.cpp

Go to the documentation of this file.

#include "Module.h"

#include <io.h>
#include <fcntl.h>
#include <share.h>
#include <sys/stat.h>

Mcu::Mcu(Bus* prmBus, Logger* prmLogger) : Module(prmBus)
{
        logger = prmLogger;

        // load config  
        int f;
        if (_sopen_s(&f, "motion.dat", _O_RDONLY | _O_BINARY, _SH_DENYNO, _S_IREAD) != 0)
        {
                logger->Log("Module::MCU could not load motion.dat", Logger::Level::Fatal, errno);
        }
        else
        {
                _lseek(f, SEEK_SET, 0L);
                _read(f, &Locations, sizeof(Locations));
                _close(f);
        }
        current = {0, 0, 0, 0};

        emergencyStopGetPtr = bus->GetComponentCurrentStatePtr("CE4DED74-F759-484F-B5AD-69C03AB7A8BD");
}

void Mcu::FrameMove(float fElapsedTime)
{
        // 0 is off, safe
        // 1 is caution lights on, wait 5 seconds
        // 2 is motion power on, wait 5 seconds
        // 3 is load mid-level, wait 5 seconds
        // 4 is motion running, watch emer stop
        // 5 is load mid-level, wait 5 seconds
        // 6 is load off-level, wait 5 seconds
        // 7 is motion power off, wait 5 seconds
        // 8 is caution lights off, set back to 0

        Command command;
        for (UINT i = 0; i < bus->commandStream.size(); i++)
        {
                command = bus->commandStream.at(i);
                if (command.delay != 0.0f) continue;

                if (command.name == "MotionStartStop")
                {
                        if (sequencer == 0 && *emergencyStopGetPtr == 1.0f) 
                        {
                                sequencer = 1;
                                timer = 0.0f;
                        }
                        else if (sequencer <= 4) 
                        {
                                sequencer = 5;
                                timer = 0.0f;
                        }
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "MotionStart")
                {
                        if (sequencer == 0 && *emergencyStopGetPtr == 1.0f) 
                        {
                                sequencer = 1;
                                timer = 0.0f;
                        }
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "MotionStop")
                {
                        if (sequencer <= 4) 
                        {
                                sequencer = 5;
                                timer = 0.0f;
                        }
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "MotionTest")
                {
                        testmode = command.ivalue;
                        if (testmode == 0)
                        {
                                bus->NormalAcceleration = D3DXVECTOR3(0, 0, 0);
                        }
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
        }

        if (*emergencyStopGetPtr != 1.0f && sequencer && sequencer < 5)
        {
                // play cool sound effect
                command.name = "MCURideHalted";
                bus->commandStream.push_back(command);
                sequencer = 5;
                timer = 0.0f; // shut it down;
        }

        if (testmode == 1) // heave UP 4G
        {
                bus->NormalAcceleration = D3DXVECTOR3(0, -0.03922660011455540164454186719264f, 0);
        }
        else if (testmode == 2) // tilt left 4G
        {
                bus->NormalAcceleration = D3DXVECTOR3(-0.03922660011455540164454186719264f, 0, 0);
        }
        else if (testmode == 3) // tilt forward 4G
        {
                bus->NormalAcceleration = D3DXVECTOR3(0, 0, 0.03922660011455540164454186719264f);
        }


        //float roll = powf(bus->RollControl, 4.0f);
        //if (bus->RollControl > 0) roll = -roll;
        //float pitch = powf(bus->PitchControl, 4.0f);
        //if (bus->PitchControl < 0) pitch = -pitch;

        //static float roll=0.0f;
        //static float pitch=0.0f;
        //static float timeSinceLastChange = 0.0f;
        //timeSinceLastChange += fElapsedTime;
        //if (timeSinceLastChange > 15.0f)
        //{
        //      timeSinceLastChange -= 15.0f;
        //      roll = (float)rand() / (float)RAND_MAX - 0.5f; // 0 to 1 becomes -.5 to +.5
        //      pitch = (float)rand() / (float)RAND_MAX - 0.5f; // 0 to 1 becomes -.5 to +.5
        //}

#pragma region Platform Responds to NormalAccelleration
        static float counteri = 0.0f, counterj = 0.0f, counterk = 0.0f; // this is the 
        D3DXVECTOR3 limitacc = bus->NormalAcceleration * 101.971621f; // convert km/s to g's
        limitacc.y -= 1.0f; // we always get 1g for free!

        // set up for 4G's based on http://msis.jsc.nasa.gov/images/section03/image106.gif
        if (limitacc.x > 4.0f)
                limitacc.x = 4.0f;
        else if (limitacc.x < -4.0f)
                limitacc.x = -4.0f;
        if (limitacc.y < -4.0f)
                limitacc.y = -4.0f;
        else if (limitacc.y > 4.0f) // quarter travel into seat
                limitacc.y = 4.0f;
        if (limitacc.z > 4.0f) // quarter travel into headrest
                limitacc.z = 4.0f;
        else if (limitacc.z < -4.0f)
                limitacc.z = -4.0f;

        float igs = limitacc.x - counteri;
        float jgs = limitacc.y - counterj;
        float kgs = limitacc.z - counterk;

        if (!testmode)
        {
                counteri += igs * fElapsedTime * 0.1f;
                counterj += jgs * fElapsedTime * 0.1f;
                counterk += kgs * fElapsedTime * 0.1f;
        }

        igs *= 0.25f; // scale to 1
        jgs *= 0.25f; // scale to 1
        kgs *= 0.25f; // scale to 1

        float roll = igs;
        float pitch = -kgs;
        float heave = -jgs; // -1.0f to 1.0f is -4.0G to +4.0G

        float currentPressure = 0.0f;
        Nullable<float> pneumaticPressure = bus->GetComponentStateFloat("0E1C068F-F901-4C1A-AC12-10F53E894579");
        if (pneumaticPressure.is_set())
                currentPressure = pneumaticPressure.get();
        float modulate = currentPressure / 50.0f - 1.0f; // 100/50 is 1, 50/50 is 0
        if (modulate > 1.0f) modulate = 1.0f;
        else if (modulate < 0.0f) modulate = 0.0f;
        roll *= modulate;
        pitch *= modulate;
        heave *= modulate;

#pragma endregion


        timer -= fElapsedTime;
        if (timer <= 0.0f)
        {
                timer = 0.0f;
                switch (sequencer)
                {
                case 1: // caution lights, wait 5 seconds
                        logger->Log("Module::MCU Sequencer 1");
                        command.name = "MotionPlatformCautionLights";
                        command.ivalue = 1;
                        command.delay = 0.0f;
                        bus->commandStream.push_back(command);
                        timer = 5.0f;
                        sequencer++;
                        // load off before we engage motion power
                        current.fl = current.fr = current.bl = current.br = 0.0f; // off-level
                        break;
                case 2: // motion power, wait 5 seconds
                        logger->Log("Module::MCU Sequencer 2");
                        command.name = "Motion Power";
                        command.ivalue = 1;
                        command.delay = 0.0f;
                        bus->commandStream.push_back(command);
                        timer = 5.0f;
                        sequencer++;
                        break;
                case 3: // load mid-level, wait 5 seconds
                        logger->Log("Module::MCU Sequencer 3");
                        current = Locations[5]; // mid-level
                        timer = 10.0f;
                        sequencer++;
                        break;
                case 4: // operating 
                        // -1 and -1 is FL, -1 and 0 is FF, -1 and 1 is FR
                        // 0 and -1 is LL, 0 and 0 is MID, 0 and 1 is RR
                        // 1 and -1 is BL, 1 and 0 is BB, 1 and 1 is BR
                        if (roll <= 0.0f && pitch <= 0.0f)
                        {
                                float newflF = 0.0f, newfrF = 0.0f, newblF = 0.0f, newbrF = 0.0f;
                                // left and right of zone 1 front side
                                newflF += Locations[1].fl * fabs(roll); // was -1 to 0, now 1 to 0, FL
                                newfrF += Locations[1].fr * fabs(roll);
                                newblF += Locations[1].bl * fabs(roll);
                                newbrF += Locations[1].br * fabs(roll);
                                newflF += Locations[2].fl * (roll + 1.0f); // was -1 to 0, now 0 to 1, FF
                                newfrF += Locations[2].fr * (roll + 1.0f);
                                newblF += Locations[2].bl * (roll + 1.0f);
                                newbrF += Locations[2].br * (roll + 1.0f);
                                float newflM = 0.0f, newfrM = 0.0f, newblM = 0.0f, newbrM = 0.0f;
                                // left and right of zone mid-line
                                newflM += Locations[4].fl * fabs(roll); // was -1 to 0, now 1 to 0, LL
                                newfrM += Locations[4].fr * fabs(roll);
                                newblM += Locations[4].bl * fabs(roll);
                                newbrM += Locations[4].br * fabs(roll);
                                newflM += Locations[5].fl * (roll + 1.0f); // was -1 to 0, now 0 to 1, MID
                                newfrM += Locations[5].fr * (roll + 1.0f);
                                newblM += Locations[5].bl * (roll + 1.0f);
                                newbrM += Locations[5].br * (roll + 1.0f);
                                // front and back of zone
                                current.fl = newflF * fabs(pitch) + newflM * (pitch + 1.0f);
                                current.fr = newfrF * fabs(pitch) + newfrM * (pitch + 1.0f);
                                current.bl = newblF * fabs(pitch) + newblM * (pitch + 1.0f);
                                current.br = newbrF * fabs(pitch) + newbrM * (pitch + 1.0f);
                        }
                        else if (roll >= 0.0f && pitch <= 0.0f)
                        {
                                float newflF = 0.0f, newfrF = 0.0f, newblF = 0.0f, newbrF = 0.0f;
                                // left and right of zone 2 front side
                                newflF += Locations[3].fl * roll; // was 0 to 1 FR
                                newfrF += Locations[3].fr * roll;
                                newblF += Locations[3].bl * roll;
                                newbrF += Locations[3].br * roll;
                                newflF += Locations[2].fl * 1.0f - roll; // was 0 to 1, now 1 to 0, FF
                                newfrF += Locations[2].fr * 1.0f - roll;
                                newblF += Locations[2].bl * 1.0f - roll;
                                newbrF += Locations[2].br * 1.0f - roll;
                                float newflM = 0.0f, newfrM = 0.0f, newblM = 0.0f, newbrM = 0.0f;
                                // left and right of zone mid-line
                                newflM += Locations[6].fl * roll; // was 0 to 1, RR
                                newfrM += Locations[6].fr * roll;
                                newblM += Locations[6].bl * roll;
                                newbrM += Locations[6].br * roll;
                                newflM += Locations[5].fl * 1.0f - roll; // was 0 to 1, now 1 to 0, MID
                                newfrM += Locations[5].fr * 1.0f - roll;
                                newblM += Locations[5].bl * 1.0f - roll;
                                newbrM += Locations[5].br * 1.0f - roll;
                                // front and back of zone
                                current.fl = newflF * fabs(pitch) + newflM * (pitch + 1.0f);
                                current.fr = newfrF * fabs(pitch) + newfrM * (pitch + 1.0f);
                                current.bl = newblF * fabs(pitch) + newblM * (pitch + 1.0f);
                                current.br = newbrF * fabs(pitch) + newbrM * (pitch + 1.0f);
                        }
                        else if (roll <= 0.0f && pitch >= 0.0f)
                        {
                                float newflF = 0.0f, newfrF = 0.0f, newblF = 0.0f, newbrF = 0.0f;
                                // left and right of zone 3 back side
                                newflF += Locations[7].fl * fabs(roll); // was -1 to 0, now 1 to 0, FL
                                newfrF += Locations[7].fr * fabs(roll);
                                newblF += Locations[7].bl * fabs(roll);
                                newbrF += Locations[7].br * fabs(roll);
                                newflF += Locations[8].fl * (roll + 1.0f); // was -1 to 0, now 0 to 1, FF
                                newfrF += Locations[8].fr * (roll + 1.0f);
                                newblF += Locations[8].bl * (roll + 1.0f);
                                newbrF += Locations[8].br * (roll + 1.0f);
                                float newflM = 0.0f, newfrM = 0.0f, newblM = 0.0f, newbrM = 0.0f;
                                // left and right of zone mid-line
                                newflM += Locations[4].fl * fabs(roll); // was -1 to 0, now 1 to 0, LL
                                newfrM += Locations[4].fr * fabs(roll);
                                newblM += Locations[4].bl * fabs(roll);
                                newbrM += Locations[4].br * fabs(roll);
                                newflM += Locations[5].fl * (roll + 1.0f); // was -1 to 0, now 0 to 1, MID
                                newfrM += Locations[5].fr * (roll + 1.0f);
                                newblM += Locations[5].bl * (roll + 1.0f);
                                newbrM += Locations[5].br * (roll + 1.0f);
                                // front and back of zone
                                current.fl = newflF * pitch + newflM * (1.0f - pitch);
                                current.fr = newfrF * pitch + newfrM * (1.0f - pitch);
                                current.bl = newblF * pitch + newblM * (1.0f - pitch);
                                current.br = newbrF * pitch + newbrM * (1.0f - pitch);
                        }
                        else if (roll >= 0.0f && pitch >= 0.0f)
                        {
                                float newflF = 0.0f, newfrF = 0.0f, newblF = 0.0f, newbrF = 0.0f;
                                // left and right of zone 4 back side
                                newflF += Locations[9].fl * roll; // was 0 to 1 FR
                                newfrF += Locations[9].fr * roll;
                                newblF += Locations[9].bl * roll;
                                newbrF += Locations[9].br * roll;
                                newflF += Locations[8].fl * 1.0f - roll; // was 0 to 1, now 1 to 0, FF
                                newfrF += Locations[8].fr * 1.0f - roll;
                                newblF += Locations[8].bl * 1.0f - roll;
                                newbrF += Locations[8].br * 1.0f - roll;
                                float newflM = 0.0f, newfrM = 0.0f, newblM = 0.0f, newbrM = 0.0f;
                                // left and right of zone mid-line
                                newflM += Locations[6].fl * roll; // was 0 to 1, RR
                                newfrM += Locations[6].fr * roll;
                                newblM += Locations[6].bl * roll;
                                newbrM += Locations[6].br * roll;
                                newflM += Locations[5].fl * 1.0f - roll; // was 0 to 1, now 1 to 0, MID
                                newfrM += Locations[5].fr * 1.0f - roll;
                                newblM += Locations[5].bl * 1.0f - roll;
                                newbrM += Locations[5].br * 1.0f - roll;
                                // front and back of zone
                                current.fl = newflF * pitch + newflM * (1.0f - pitch);
                                current.fr = newfrF * pitch + newfrM * (1.0f - pitch);
                                current.bl = newblF * pitch + newblM * (1.0f - pitch);
                                current.br = newbrF * pitch + newbrM * (1.0f - pitch);
                        }
                        {
                                float mag = max(fabsf(pitch), fabsf(roll)); // say pitch is 0.0f, roll is -0.9f, mag is 0.9f
                                if (mag < 1.0f)
                                {
                                        heave *= (1.0f - mag); // so if we have 0.1 avail 1.0f-0.9f = 0.1f available
                                        if (heave > 0)
                                        {
                                                // high - mid is how much room we have... 
                                                current.fl += (Locations[10].fl - Locations[5].fl) * heave;
                                                current.fr += (Locations[10].fr - Locations[5].fr) * heave;
                                                current.bl += (Locations[10].bl - Locations[5].bl) * heave;
                                                current.br += (Locations[10].br - Locations[5].br) * heave;
                                        }
                                        else
                                        {
                                                // FL was 0.5
                                                // (0.5 - 0.0) = 0.5 to go
                                                // 0.5 * -1.0 = -0.5
                                                current.fl += (Locations[5].fl - Locations[0].fl) * heave * 0.75f; 
                                                current.fr += (Locations[5].fr - Locations[0].fr) * heave * 0.75f; 
                                                current.bl += (Locations[5].bl - Locations[0].bl) * heave * 0.75f; 
                                                current.br += (Locations[5].br - Locations[0].br) * heave * 0.75f; 
                                        }
                                }
                        }

                        break;
                case 5: // load mid-level, wait 5 seconds
                        logger->Log("Module::MCU Sequencer 5");
                        current = Locations[5]; // mid-level
                        timer = 10.0f;
                        sequencer++;
                        break;
                case 6: // load off-level, wait 5 seconds
                        logger->Log("Module::MCU Sequencer 6");
                        current.fl = current.fr = current.bl = current.br = 0.0f; // off-level
                        timer = 10.0f;
                        sequencer++;
                        break;
                case 7: // motion power, wait 5 seconds
                        logger->Log("Module::MCU Sequencer 7");
                        command.name = "Motion Power";
                        command.ivalue = 0;
                        command.delay = 0.0f;
                        bus->commandStream.push_back(command);
                        timer = 5.0f;
                        sequencer++;
                        break;
                case 8: // caution lights, wait 5 seconds
                        logger->Log("Module::MCU Sequencer 8");
                        command.name = "MotionPlatformCautionLights";
                        command.ivalue = 0;
                        command.delay = 0.0f;
                        bus->commandStream.push_back(command);
                        sequencer = 0;
                        break;
                }
        }

        bus->PlatformBackLeftBellow = current.bl;
        bus->PlatformBackRightBellow = current.br;
        bus->PlatformFrontLeftBellow = current.fl;
        bus->PlatformFrontRightBellow = current.fr;
        bus->PlatformMotionSequencer = sequencer;
}