void HandleMessages( CCommand& commandIn )
    {
        if( commandIn.Equals( "reportSetting" ) )
        {
            Serial.print( F( "*settings:" ) );
            Serial.print( F( "smoothingIncriment|" ) );
            Serial.print( String( NConfigManager::m_throttleSmoothingIncrement ) + "," );
            Serial.print( F( "deadZone_min|" ) );
            Serial.print( String( NConfigManager::m_deadZoneMin ) + "," );
            Serial.print( F( "deadZone_max|" ) );
            Serial.print( String( NConfigManager::m_deadZoneMax ) + ";" );
            Serial.print( F( "water_type" ) );
            Serial.println( String( NConfigManager::m_waterType ) + ";" );

        }
        else if( commandIn.Equals( "rcap" ) ) //report capabilities
        {
            Serial.print( F( "CAPA:" ) );
            Serial.print( m_capabilityBitmask );
            Serial.print( ';' );
            NArduinoManager::ScanI2C();
        }
        else if( commandIn.Equals( "updateSetting" ) )
        {
            //TODO: Need to update the motors with new deadZone setting. Probably move
            //deadzone to the thruster resposibilitiy
            NConfigManager::m_throttleSmoothingIncrement = commandIn.m_arguments[1];
            NConfigManager::m_deadZoneMin					= commandIn.m_arguments[2];
            NConfigManager::m_deadZoneMax		= commandIn.m_arguments[3];
            NConfigManager::m_waterType		= commandIn.m_arguments[4];
        }
    }
void CExternalLights::Update( CCommand& commandIn )
{
    if( commandIn.Equals( "eligt" ) )
	{
		float percentValue = ( float )commandIn.m_arguments[1] / 100.0f;
		int value = (int)( 255.0f * percentValue );
		
		elight.Write( value );
		
		Serial.print( F( "LIGTE:" ) );
		Serial.print( value );
		Serial.print( ';' );
		
		Serial.print( F( "LIGPE:" ) );
		Serial.print( percentValue );
		Serial.println( ';' );
	}
}
예제 #3
0
void CBNO055::Update( CCommand& commandIn )
{
	if( commandIn.Equals( "imumode" ) )
	{
		if( initalized && commandIn.m_arguments[ 0 ] != 0 )
		{
			if( commandIn.m_arguments[ 1 ] == 0 )
			{
				// Turn off override
				inFusionMode	= true;
				bno.EnterNDOFMode();

			}

			if( commandIn.m_arguments[ 1 ] == 12 )
			{
				// Override to NDOF
				inFusionMode	= true;
				bno.EnterNDOFMode();
			}

			if( commandIn.m_arguments[ 1 ] == 8 )
			{
				// Override to IMU mode
				inFusionMode	= false;
				bno.EnterIMUMode();
			}
		}
		else
		{
			Serial.println( "log:Can't enter override, IMU is not initialized yet!;" );
		}
	}

	// 1000 / 21
	if( bno055_sample_timer.HasElapsed( 47 ) )
	{
		if( !initalized )
		{
			// Attempt every 10 secs
			if( report_timer.HasElapsed( 10000 ) )
			{
				// Attempt to initialize the chip again
				InitializeSensor();
			}

			return;
		}

		// System status checks
		if( report_timer.HasElapsed( 1000 ) )
		{
			// System calibration
			if( bno.GetCalibration() )
			{
				Serial.print( "BNO055.CALIB_MAG:" );
				Serial.print( bno.m_magCal );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_ACC:" );
				Serial.print( bno.m_accelCal );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_GYR:" );
				Serial.print( bno.m_gyroCal );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_SYS:" );
				Serial.print( bno.m_systemCal );
				Serial.println( ';' );

				// Get offsets
				bno.GetGyroOffsets();
				bno.GetAccelerometerOffsets();
				bno.GetMagnetometerOffsets();
			}
			else
			{
				Serial.print( "BNO055.CALIB_MAG:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_ACC:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_GYR:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_SYS:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
			}

			// Operating mode
			if( bno.GetOperatingMode() )
			{
				Serial.print( "BNO055.MODE:" );
				Serial.print( bno.m_operatingMode );
				Serial.println( ';' );
			}
			else
			{
				Serial.print( "BNO055.MODE:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
			}

			// System status
			if( bno.GetSystemStatus() )
			{
				Serial.print( "BNO055_STATUS:" );
				Serial.print( bno.m_systemStatus, HEX );
				Serial.println( ";" );
			}
			else
			{
				Serial.print( "BNO055_STATUS:" );
				Serial.print( "N/A" );
				Serial.println( ";" );
			}

			// System Error
			if( bno.GetSystemError() )
			{
				Serial.print( "BNO055_ERROR_FLAG:" );
				Serial.print( bno.m_systemError );
				Serial.println( ";" );
			}
			else
			{
				Serial.print( "BNO055_ERROR_FLAG:" );
				Serial.print( "N/A" );
				Serial.println( ";" );
			}

		}

		// Get orientation data
        if( bno.GetVector( CAdaBNO055::VECTOR_EULER, euler ) )
        {			
            // Throw out exactly zero heading values that are all zeroes - necessary when switching modes
            if( euler.x() != 0.0f  )
            {
                // These may need adjusting
                
                NDataManager::m_navData.YAW		= euler.x();
                NDataManager::m_navData.HDGD	= euler.x();
            }
			
			NDataManager::m_navData.PITC	= euler.z();
			NDataManager::m_navData.ROLL	= -euler.y();
        }
	}
}
void CDeadManSwitch::Update( CCommand& command )
{
    // Check for messages
    if( NCommManager::m_isCommandAvailable )
    {
        if( command.Equals( "ping" ) )
        {
            deadmanSwitchTimer.Reset();

            if( _deadmanSwitchEnabled )
            {
                int argsToSend[] = {0};
                command.PushCommand( "start", argsToSend );
                _deadmanSwitchEnabled = false;
            }

            Serial.print( F( "pong:" ) );
            Serial.print( command.m_arguments[0] );
            Serial.print( "," );
            Serial.print( millis() );
            Serial.print( ";" );
        }
        else if( command.Equals( "dms" ) )
        {
            if( command.m_arguments[0] == 0 )
            {
                _deadmanSwitchArmed = false;
            }
            else
            {
                _deadmanSwitchArmed = true;
            }
        }
    }

    //Auto arm the deadman switch after a reasonable boot time
    if( !_deadmanSwitchArmed && ( millis() > DEADMAN_SWITCH_DELAY_TO_ARM_MS ) )
    {
        _deadmanSwitchArmed = true;
    }

    if( ( deadmanSwitchTimer.HasElapsed( 2000 ) ) && _deadmanSwitchArmed && ( _deadmanSwitchEnabled == false ) )
    {
        int argsToSend[] = {0}; //include number of parms as fist parm
        command.PushCommand( "deptloff", argsToSend );
        command.PushCommand( "headloff", argsToSend );
        command.PushCommand( "stop", argsToSend );
        _deadmanSwitchEnabled = true;
    }

    if( _deadmanSwitchEnabled && blinklightTimer.HasElapsed( 500 ) )
    {
        int argsToSend[] = {1, 50};

        if( blinkstate )
        {
            argsToSend[1] = 0;
        }

        command.PushCommand( "ligt", argsToSend );
        blinkstate = !blinkstate;
    }
}
void CThrusters::Update( CCommand& command )
{
    if( command.Equals( "mtrmod1" ) )
    {
        port_motor.m_positiveModifier = command.m_arguments[1] / 100;
        vertical_motor.m_positiveModifier = command.m_arguments[2] / 100;
        starboard_motor.m_positiveModifier = command.m_arguments[3] / 100;
    }

    if( command.Equals( "mtrmod2" ) )
    {
        port_motor.m_negativeModifier = command.m_arguments[1] / 100;
        vertical_motor.m_negativeModifier = command.m_arguments[2] / 100;
        starboard_motor.m_negativeModifier = command.m_arguments[3] / 100;
    }

    if( command.Equals( "rmtrmod" ) )
    {
        Serial.print( F( "mtrmod:" ) );
        Serial.print( port_motor.m_positiveModifier );
        Serial.print( "," );
        Serial.print( vertical_motor.m_positiveModifier );
        Serial.print( "," );
        Serial.print( starboard_motor.m_positiveModifier );
        Serial.print( "," );
        Serial.print( port_motor.m_negativeModifier );
        Serial.print( "," );
        Serial.print( vertical_motor.m_negativeModifier );
        Serial.print( "," );
        Serial.print( starboard_motor.m_negativeModifier );
        Serial.println( ";" );
    }

    if( command.Equals( "go" ) )
    {
        //ignore corrupt data
        if( command.m_arguments[1] > 999 && command.m_arguments[2] > 999 && command.m_arguments[3] > 999 && command.m_arguments[1] < 2001 && command.m_arguments[2] < 2001 && command.m_arguments[3] < 2001 )
        {
            p = command.m_arguments[1];
            v = command.m_arguments[2];
            s = command.m_arguments[3];

            if( command.m_arguments[4] == 1 )
            {
                bypasssmoothing = true;
            }
        }
    }

    if( command.Equals( "port" ) )
    {
        //ignore corrupt data
        if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
        {
            p = command.m_arguments[1];

            if( command.m_arguments[2] == 1 )
            {
                bypasssmoothing = true;
            }
        }
    }

    if( command.Equals( "vertical" ) )
    {
        //ignore corrupt data
        if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
        {
            v = command.m_arguments[1];

            if( command.m_arguments[2] == 1 )
            {
                bypasssmoothing = true;
            }
        }
    }

    if( command.Equals( "starboard" ) )
    {
        //ignore corrupt data
        if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
        {
            s = command.m_arguments[1];

            if( command.m_arguments[2] == 1 )
            {
                bypasssmoothing = true;
            }
        }
    }

    if( command.Equals( "thro" ) || command.Equals( "yaw" ) )
    {
        if( command.Equals( "thro" ) )
        {
            if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 )
            {
                trg_throttle = command.m_arguments[1] / 100.0;
            }
        }

        if( command.Equals( "yaw" ) )
        {
            //ignore corrupt data
            if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 ) //percent of max turn
            {
                trg_yaw = command.m_arguments[1] / 100.0;
            }
        }

        // The code below spreads the throttle spectrum over the possible range
        // of the motor. Not sure this belongs here or should be placed with
        // deadzon calculation in the motor code.
        if( trg_throttle >= 0 )
        {
            p = 1500 + ( 500.0 / abs( port_motor.m_positiveModifier ) ) * trg_throttle;
            s = p;
        }
        else
        {
            p = 1500 + ( 500.0 / abs( port_motor.m_negativeModifier ) ) * trg_throttle;
            s = p;
        }

        trg_motor_power = s;

        int turn = trg_yaw * 250; //max range due to reverse range

        if( trg_throttle >= 0 )
        {
            int offset = ( abs( turn ) + trg_motor_power ) - 2000;

            if( offset < 0 )
            {
                offset = 0;
            }

            p = trg_motor_power + turn - offset;
            s = trg_motor_power - turn - offset;
        }
        else
        {
            int offset = 1000 - ( trg_motor_power - abs( turn ) );

            if( offset < 0 )
            {
                offset = 0;
            }

            p = trg_motor_power + turn + offset;
            s = trg_motor_power - turn + offset;
        }

    }

    if( command.Equals( "lift" ) )
    {
        if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 )
        {
            trg_lift = command.m_arguments[1] / 100.0;
            v = 1500 + 500 * trg_lift;
        }
    }


    #ifdef ESCPOWER_PIN
    else if( command.Equals( "escp" ) )
    {
        escpower.Write( command.m_arguments[1] ); //Turn on the ESCs
        Serial.print( F( "log:escpower=" ) );
        Serial.print( command.m_arguments[1] );
        Serial.println( ';' );
    }

    #endif
    else if( command.Equals( "start" ) )
    {
        port_motor.Activate();
        vertical_motor.Activate();
        starboard_motor.Activate();
    }
    else if( command.Equals( "stop" ) )
    {
        p = MOTOR_TARGET_NEUTRAL_US;
        v = MOTOR_TARGET_NEUTRAL_US;
        s = MOTOR_TARGET_NEUTRAL_US;
        // Not sure why the reset does not re-attach the servo.
        //port_motor.stop();
        //vertical_motor.stop();
        //starboard_motor.stop();
    }

    #ifdef ESCPOWER_PIN
    else if( ( command.Equals( "mcal" ) ) && ( canPowerESCs ) )
    {
        Serial.println( F( "log:Motor Callibration Staring;" ) );
        //Experimental. Add calibration code here
        Serial.println( F( "log:Motor Callibration Complete;" ) );
    }

    #endif

    //to reduce AMP spikes, smooth large power adjustments out. This incirmentally adjusts the motors and servo
    //to their new positions in increments.  The incriment should eventually be adjustable from the cockpit so that
    //the pilot could have more aggressive response profiles for the ROV.
    if( controltime.HasElapsed( 50 ) )
    {
        if( p != new_p || v != new_v || s != new_s )
        {
            new_p = p;
            new_v = v;
            new_s = s;

            // Check to see if any motors are non-neutral to signal system that at least one motor is running
            if( p != MOTOR_TARGET_NEUTRAL_US || v != MOTOR_TARGET_NEUTRAL_US || s != MOTOR_TARGET_NEUTRAL_US )
            {
                NDataManager::m_thrusterData.MotorsActive = true;
            }
            else
            {
                NDataManager::m_thrusterData.MotorsActive = false;
            }

            Serial.print( F( "motors:" ) );
            Serial.print( port_motor.SetMotorTarget( new_p ) );
            Serial.print( ',' );
            Serial.print( vertical_motor.SetMotorTarget( new_v ) );
            Serial.print( ',' );
            Serial.print( starboard_motor.SetMotorTarget( new_s ) );
            Serial.println( ';' );
        }

    }

    NDataManager::m_navData.FTHR = map( ( new_p + new_s ) / 2, 1000, 2000, -100, 100 );

    //The output from the motors is unique to the thruster configuration
    if( thrusterOutput.HasElapsed( 1000 ) )
    {
        Serial.print( F( "mtarg:" ) );
        Serial.print( p );
        Serial.print( ',' );
        Serial.print( v );
        Serial.print( ',' );
        Serial.print( s );
        Serial.println( ';' );
        NDataManager::m_thrusterData.MATC = port_motor.IsActive() || port_motor.IsActive() || port_motor.IsActive();
        Serial.print( F( "mtrmod:" ) );
        Serial.print( port_motor.m_positiveModifier );
        Serial.print( "," );
        Serial.print( vertical_motor.m_positiveModifier );
        Serial.print( "," );
        Serial.print( starboard_motor.m_positiveModifier );
        Serial.print( "," );
        Serial.print( port_motor.m_negativeModifier );
        Serial.print( "," );
        Serial.print( vertical_motor.m_negativeModifier );
        Serial.print( "," );
        Serial.print( starboard_motor.m_negativeModifier );
        Serial.println( ";" );
    }
}