<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/pm/p9_pm_recovery_ffdc_pgpe.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
// *INDENT-OFF*

///
/// @file   p9_pm_recovery_ffdc_pgpe.C
/// @brief  Models PGPE platform for the FFDC collection of PM complex
///
/// *HWP HWP Owner:      Greg Still <stillgs@us.ibm.com>
/// *HWP FW Owner:       Prem S Jha <premjha2@in.ibm.com>
/// *HWP Team:           PM
/// *HWP Level:          2
/// *HWP Consumed by:    Hostboot
//
// *INDENT-OFF*
//--------------------------------------------------------------------------
// Includes
//--------------------------------------------------------------------------

#include <p9_pm_recovery_ffdc_pgpe.H>
#include <p9_hcd_memmap_occ_sram.H>
#include <p9_ppe_defs.H>
#include <stddef.h>
#include <endian.h>

 namespace p9_stop_recov_ffdc
 {
    PlatPgpe::PlatPgpe( const fapi2::Target< fapi2::TARGET_TYPE_PROC_CHIP > i_procChipTgt )
      : PlatPmComplex( i_procChipTgt,
                       OCC_SRAM_PGPE_HEADER_ADDR,
                       OCC_SRAM_PGPE_TRACE_START,
                       OCC_SRAM_PGPE_DASHBOARD_START,
                       PLAT_PGPE )
    { }

    //----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::collectFfdc( void * i_pHomerBuf )
    {
        FAPI_DBG(">> PlatPgpe::collectFfdc");
        fapi2::current_err = fapi2::FAPI2_RC_SUCCESS;
        fapi2::ReturnCode l_retCode = fapi2::FAPI2_RC_SUCCESS;
        uint8_t l_ffdcValdityVect = PPE_FFDC_ALL_VALID;

        uint8_t l_haltState = PPE_HALT_COND_UNKNOWN;
        uint8_t *l_pFfdcLoc = NULL;
        HomerFfdcRegion * l_pHomerFfdc =
                ( HomerFfdcRegion *)( (uint8_t *)i_pHomerBuf + FFDC_REGION_HOMER_BASE_OFFSET );

        l_pFfdcLoc = (uint8_t *)(&l_pHomerFfdc->iv_pgpeFfdcRegion);

        //In case of error , invalidate FFDC in header.

        l_retCode = collectPpeState ( PGPE_BASE_ADDRESS,
                                      l_pFfdcLoc );
        if ( l_retCode != fapi2::FAPI2_RC_SUCCESS )
        {
            FAPI_ERR ( "Error collecting PGPE State" );
            l_ffdcValdityVect &= ~PPE_STATE_VALID;
        }

        l_retCode = collectTrace( l_pFfdcLoc );

        if( l_retCode )
        {
            FAPI_ERR("Error in collecting PGPE Trace " );
            l_ffdcValdityVect &= ~PPE_TRACE_VALID;
        }

        l_retCode = collectGlobals( l_pFfdcLoc );

        if( l_retCode )
        {
            FAPI_ERR("Error in collecting PGPE Globals" );
            l_ffdcValdityVect &= ~PPE_DASHBOARD_VALID;
        }

        l_retCode = collectImageHeader( l_pFfdcLoc );

        if( l_retCode )
        {
            FAPI_ERR("Error in collecting PGPE Image header" );
            l_ffdcValdityVect &= ~PPE_IMAGE_HEADER_VALID;
        }

        FAPI_TRY( updatePgpeFfdcHeader( l_pFfdcLoc, l_ffdcValdityVect, l_haltState ),
                          "Failed To Update PGPE FFDC Header for PGPE " );

        fapi_try_exit:
        FAPI_DBG("<< PlatPgpe::collectFfdc");
        return fapi2::current_err;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::collectTrace( uint8_t * i_pTraceBuf )
    {
        FAPI_DBG(">> PlatPgpe::collectTrace" );
        PpeFfdcLayout * l_pPgpeFfdc = ( PpeFfdcLayout *) ( i_pTraceBuf );

        uint8_t * l_pTraceLoc = &l_pPgpeFfdc->iv_ppeTraces[0];

        FAPI_TRY( PlatPmComplex::collectSramInfo
                    ( PlatPmComplex::getProcChip(),
                      l_pTraceLoc,
                      TRACES,
                      FFDC_PPE_TRACES_SIZE ),
                  "Trace Collection Failed" );

        fapi_try_exit:
        FAPI_DBG("<< PlatPgpe::collectTrace" );
        return fapi2::current_err;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode  PlatPgpe::collectGlobals( uint8_t * i_pPgpeGlobals )
    {
        FAPI_DBG(">> PlatPgpe::collectGlobals" );
        PpeFfdcLayout * l_pPgpeFfdc = ( PpeFfdcLayout *) ( i_pPgpeGlobals );
        uint8_t * l_pTraceLoc = &l_pPgpeFfdc->iv_ppeGlobals[0];

        FAPI_TRY( PlatPmComplex::collectSramInfo
                    ( PlatPmComplex::getProcChip(),
                      l_pTraceLoc,
                      DASH_BOARD_VAR,
                      OCC_SRAM_PGPE_DASHBOARD_SIZE ),
                  "Failed To Collect PGPE Global Variables" );

        fapi_try_exit:
        FAPI_DBG("<< PlatPgpe::collectGlobals" );
        return fapi2::current_err;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::collectInternalReg( uint8_t * i_pPgpeIntReg )
    {
        return fapi2::FAPI2_RC_SUCCESS;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::collectImageHeader( uint8_t * i_pPgpeImgHdr )
    {
        FAPI_DBG(">> PlatPgpe::collectImageHeader" );
        PpeFfdcLayout *l_pPgpeFfdc = ( PpeFfdcLayout *) ( i_pPgpeImgHdr );

        uint8_t * l_pTraceLoc = &l_pPgpeFfdc->iv_ppeImageHeader[0];
        FAPI_TRY( PlatPmComplex::collectSramInfo
                  ( PlatPmComplex::getProcChip(),
                    l_pTraceLoc,
                    IMAGE_HEADER,
                    FFDC_PPE_IMG_HDR_SIZE ),
                  "Failed To Collect PGPE Image Header" );

        fapi_try_exit:
        FAPI_DBG("<< PlatPgpe::collectImageHeader" );
        return fapi2::current_err;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::updatePgpeFfdcHeader( uint8_t * i_pHomerBuf,
                                                      bool i_ffdcValid, uint8_t i_haltState )
    {
        FAPI_DBG(">> updatePgpeFfdcHeader" );

        PpeFfdcHeader * l_pPgpeFfdcHdr       =  ( (PpeFfdcHeader *)(( PpeFfdcHdrRegion * ) i_pHomerBuf ));
        l_pPgpeFfdcHdr->iv_ppeMagicNumber    =  htobe32( FFDC_PGPE_MAGIC_NUM );
        l_pPgpeFfdcHdr->iv_ppeNumber         =  0;
        PlatPmComplex::updatePpeFfdcHeader( l_pPgpeFfdcHdr, i_ffdcValid, i_haltState );

        FAPI_DBG("<< updatePgpeFfdcHeader" );
        return fapi2::FAPI2_RC_SUCCESS;
    }
    //-----------------------------------------------------------------------

extern "C"
{
    fapi2::ReturnCode p9_pm_recovery_ffdc_pgpe( const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP >& i_procChip,
                                                void * i_pFfdcBuf )
    {
        FAPI_IMP(">> p9_pm_recovery_pgpe" );

        PlatPgpe l_pgpeFfdc( i_procChip );
        FAPI_TRY( l_pgpeFfdc.collectFfdc( i_pFfdcBuf ),
                  "Failed To Collect PGPE FFDC" );

        fapi_try_exit:
        FAPI_IMP("<< p9_pm_recovery_pgpe" );
        return fapi2::current_err;
    }

}

}//namespace p9_stop_recov_ffdc ends
<commit_msg>STOP Recovery: Misc infra. updates to enable PM FFDC in HOMER<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/pm/p9_pm_recovery_ffdc_pgpe.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
// *INDENT-OFF*

///
/// @file   p9_pm_recovery_ffdc_pgpe.C
/// @brief  Models PGPE platform for the FFDC collection of PM complex
///
/// *HWP HWP Owner:      Greg Still <stillgs@us.ibm.com>
/// *HWP FW Owner:       Prem S Jha <premjha2@in.ibm.com>
/// *HWP Team:           PM
/// *HWP Level:          2
/// *HWP Consumed by:    Hostboot
//
// *INDENT-OFF*
//--------------------------------------------------------------------------
// Includes
//--------------------------------------------------------------------------

#include <p9_pm_recovery_ffdc_pgpe.H>
#include <p9_hcd_memmap_occ_sram.H>
#include <p9_ppe_defs.H>
#include <stddef.h>
#include <endian.h>

 namespace p9_stop_recov_ffdc
 {
    PlatPgpe::PlatPgpe( const fapi2::Target< fapi2::TARGET_TYPE_PROC_CHIP > i_procChipTgt )
      : PlatPmComplex( i_procChipTgt,
                       PLAT_PGPE,
                       OCC_SRAM_PGPE_HEADER_ADDR,
                       OCC_SRAM_PGPE_TRACE_START,
                       OCC_SRAM_PGPE_DASHBOARD_START )
    { }

    //----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::init ( void* i_pHomerBuf )
    {
        FAPI_DBG (">> PlatPgpe::init" );
        FAPI_TRY ( collectFfdc( i_pHomerBuf, INIT),
                   "Failed To init PGPE FFDC" );

    fapi_try_exit:
        FAPI_DBG ("<< PlatPgpe::init" );
        return fapi2::current_err;
    }

    //----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::collectFfdc( void *  i_pHomerBuf,
                                             uint8_t i_ffdcType )
    {
        FAPI_DBG(">> PlatPgpe::collectFfdc: 0x%02X", i_ffdcType);

        fapi2::ReturnCode l_retCode = fapi2::FAPI2_RC_SUCCESS;

        HomerFfdcRegion * l_pHomerFfdc =
                ( HomerFfdcRegion *)( (uint8_t *)i_pHomerBuf + FFDC_REGION_HOMER_BASE_OFFSET );

        uint8_t* l_pFfdcLoc = (uint8_t *)(&l_pHomerFfdc->iv_pgpeFfdcRegion);
        PpeFfdcHeader* l_pPgpeFfdcHdr = (PpeFfdcHeader*) l_pFfdcLoc;
        uint16_t l_ffdcValdityVect = l_pPgpeFfdcHdr->iv_sectionsValid;

        if ( i_ffdcType & INIT )
        {   // overwrite on init
            l_ffdcValdityVect = PPE_FFDC_INVALID;
        }

        //In case of error , invalidate FFDC in header.

        if ( i_ffdcType & PPE_HALT_STATE )
        {
            l_ffdcValdityVect |= PPE_HALT_STATE_VALID;
            l_retCode = readPpeHaltState ( PGPE_BASE_ADDRESS, l_pFfdcLoc);
            if ( l_retCode != fapi2::FAPI2_RC_SUCCESS )
            {
                FAPI_ERR ( "Error collecting PGPE Halt State" );
                l_ffdcValdityVect &= ~PPE_HALT_STATE_VALID;
            }
        }

        if (i_ffdcType & PPE_STATE )
        {
            l_ffdcValdityVect |= PPE_STATE_VALID;
            l_retCode = collectPpeState ( PGPE_BASE_ADDRESS,
                                          l_pFfdcLoc );
            if ( l_retCode != fapi2::FAPI2_RC_SUCCESS )
            {
                FAPI_ERR ( "Error collecting PGPE State" );
                l_ffdcValdityVect &= ~PPE_STATE_VALID;
            }
        }

        if ( i_ffdcType & TRACES )
        {
            l_ffdcValdityVect |= PPE_TRACE_VALID;
            l_retCode = collectTrace( l_pFfdcLoc );

            if( l_retCode )
            {
                FAPI_ERR("Error in collecting PGPE Trace " );
                l_ffdcValdityVect &= ~PPE_TRACE_VALID;
            }
        }

        if ( i_ffdcType & DASH_BOARD_VAR )
        {
            l_ffdcValdityVect |= PPE_DASHBOARD_VALID;
            l_retCode = collectGlobals( l_pFfdcLoc );

            if( l_retCode )
            {
                FAPI_ERR("Error in collecting PGPE Globals" );
                l_ffdcValdityVect &= ~PPE_DASHBOARD_VALID;
            }
        }

        if ( i_ffdcType & IMAGE_HEADER )
        {
            l_ffdcValdityVect |= PPE_IMAGE_HEADER_VALID;
            l_retCode = collectImageHeader( l_pFfdcLoc );

            if( l_retCode )
            {
                FAPI_ERR("Error in collecting PGPE Image header" );
                l_ffdcValdityVect &= ~PPE_IMAGE_HEADER_VALID;
            }
        }

        FAPI_TRY( updatePgpeFfdcHeader( l_pFfdcLoc, l_ffdcValdityVect ),
                          "Failed To Update PGPE FFDC Header for PGPE " );

        if (l_ffdcValdityVect == PPE_FFDC_INVALID)
            setPmFfdcSectionValid ( i_pHomerBuf, PM_FFDC_PGPE_VALID, false );
        else
            setPmFfdcSectionValid ( i_pHomerBuf, PM_FFDC_PGPE_VALID );

    fapi_try_exit:
        FAPI_DBG("<< PlatPgpe::collectFfdc");
        return fapi2::current_err;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::collectTrace( uint8_t * i_pTraceBuf )
    {
        FAPI_DBG(">> PlatPgpe::collectTrace" );
        PpeFfdcLayout * l_pPgpeFfdc = ( PpeFfdcLayout *) ( i_pTraceBuf );

        uint8_t * l_pTraceLoc = &l_pPgpeFfdc->iv_ppeTraces[0];

        FAPI_TRY( PlatPmComplex::collectSramInfo
                    ( PlatPmComplex::getProcChip(),
                      l_pTraceLoc,
                      TRACES,
                      FFDC_PPE_TRACES_SIZE ),
                  "Trace Collection Failed" );

    fapi_try_exit:
        FAPI_DBG("<< PlatPgpe::collectTrace" );
        return fapi2::current_err;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode  PlatPgpe::collectGlobals( uint8_t * i_pPgpeGlobals )
    {
        FAPI_DBG(">> PlatPgpe::collectGlobals" );
        PpeFfdcLayout * l_pPgpeFfdc = ( PpeFfdcLayout *) ( i_pPgpeGlobals );
        uint8_t * l_pTraceLoc = &l_pPgpeFfdc->iv_ppeGlobals[0];

        FAPI_TRY( PlatPmComplex::collectSramInfo
                    ( PlatPmComplex::getProcChip(),
                      l_pTraceLoc,
                      DASH_BOARD_VAR,
                      OCC_SRAM_PGPE_DASHBOARD_SIZE ),
                  "Failed To Collect PGPE Global Variables" );

    fapi_try_exit:
        FAPI_DBG("<< PlatPgpe::collectGlobals" );
        return fapi2::current_err;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::collectInternalReg( uint8_t * i_pPgpeIntReg )
    {
        return fapi2::FAPI2_RC_SUCCESS;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::collectImageHeader( uint8_t * i_pPgpeImgHdr )
    {
        FAPI_DBG(">> PlatPgpe::collectImageHeader" );
        PpeFfdcLayout *l_pPgpeFfdc = ( PpeFfdcLayout *) ( i_pPgpeImgHdr );

        uint8_t * l_pTraceLoc = &l_pPgpeFfdc->iv_ppeImageHeader[0];
        FAPI_TRY( PlatPmComplex::collectSramInfo
                  ( PlatPmComplex::getProcChip(),
                    l_pTraceLoc,
                    IMAGE_HEADER,
                    FFDC_PPE_IMG_HDR_SIZE ),
                  "Failed To Collect PGPE Image Header" );

    fapi_try_exit:
        FAPI_DBG("<< PlatPgpe::collectImageHeader" );
        return fapi2::current_err;
    }

    //-----------------------------------------------------------------------

    fapi2::ReturnCode PlatPgpe::updatePgpeFfdcHeader( uint8_t* i_pHomerBuf,
                                                      uint16_t i_sectionsValid )
    {
        FAPI_DBG(">> updatePgpeFfdcHeader" );

        PpeFfdcHeader * l_pPgpeFfdcHdr       =  ( (PpeFfdcHeader *)(( PpeFfdcHdrRegion * ) i_pHomerBuf ));
        l_pPgpeFfdcHdr->iv_ppeMagicNumber    =  htobe32( FFDC_PGPE_MAGIC_NUM );
        l_pPgpeFfdcHdr->iv_ppeNumber         =  0;
        updatePpeFfdcHeader ( l_pPgpeFfdcHdr, i_sectionsValid );

        FAPI_DBG("<< updatePgpeFfdcHeader" );
        return fapi2::FAPI2_RC_SUCCESS;
    }
    //-----------------------------------------------------------------------

extern "C"
{
    fapi2::ReturnCode p9_pm_recovery_ffdc_pgpe( const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP >& i_procChip,
                                                void * i_pFfdcBuf )
    {
        FAPI_IMP(">> p9_pm_recovery_pgpe" );

        PlatPgpe l_pgpeFfdc( i_procChip );
        FAPI_TRY( l_pgpeFfdc.collectFfdc( i_pFfdcBuf, (ALL & ~PPE_HALT_STATE) ),
                  "Failed To Collect PGPE FFDC" );

    fapi_try_exit:
        FAPI_IMP("<< p9_pm_recovery_pgpe" );
        return fapi2::current_err;
    }

}

}//namespace p9_stop_recov_ffdc ends
<|endoftext|>


<commit_before>#include "analogjoystick.hpp"

void setupJoystick()
{
	pinMode(joystickXADC, INPUT);
	pinMode(joystickXADC, INPUT);
	pinMode(joystickEnable, INPUT);
	digitalWrite(joystickEnable, HIGH);
}
/*
void disableJoystick()
{
	digitalWrite(joystickEnable, LOW);
}
*/
void getJoystickReading(int* X, int* Y)
{
	*X = analogRead(joystickXADC);
	*Y = analogRead(joystickYADC);
}

int sign(int n)
{
	if(n > 0)
	{
		return 1;
	}
	return -1;
}

/* Get joystick position and turn it into motor velocities */
//
//   +y
//  -x +x
//    -y
//
void joystickSetMotors()
{
	int xAxis = 0;
	int yAxis = 0;

	const int XMiddle = 1023 / 2;
	const int YMiddle = 1023 / 2;

	getJoystickReading(&xAxis, &yAxis);

	//shift 0
	xAxis -= XMiddle;
	yAxis -= YMiddle;

	//make x go the right dir
	//xAxis *= -1;

	/* Apply deadzones */
	if (abs(xAxis) < DEAD_ZONE)
	{
		xAxis = 0;
	}
	if (abs(yAxis) < DEAD_ZONE)
	{
		yAxis = 0;
	}

	/* Calculate drive outputs -- magic*/
	//int leftVelocity = .5*float(yAxis) + 2 * sign(xAxis) * sqrt(abs(xAxis));
	//int rightVelocity = .5*float(yAxis) - 2 * sign(xAxis) * sqrt(abs(xAxis));
	int leftVelocity = yAxis + 3 * sign(xAxis) * sqrt(abs(xAxis));
	int rightVelocity = yAxis - 3 * sign(xAxis) * sqrt(abs(xAxis));

	/* Limit drive velcocities to vaild values and convert them to drive speeds and directions */

	byte leftSpeed = min(abs(leftVelocity), 255);
	byte leftDir = (leftVelocity < 0) ? MC_MOTOR_REVERSE : MC_MOTOR_FORWARD;
	byte rightSpeed = min(abs(rightVelocity), 255);
	byte rightDir = (rightVelocity < 0) ? MC_MOTOR_REVERSE : MC_MOTOR_FORWARD;

	setLeftMotorDutyCycle(leftDir, leftSpeed);
	setRightMotorDutyCycle(rightDir, rightSpeed);

}
<commit_msg>John's joystick code<commit_after>#include "analogjoystick.hpp"

void setupJoystick()
{
	pinMode(joystickXADC, INPUT);
	pinMode(joystickXADC, INPUT);
	pinMode(joystickEnable, INPUT);
	digitalWrite(joystickEnable, HIGH);
}
/*
void disableJoystick()
{
	digitalWrite(joystickEnable, LOW);
}
*/
void getJoystickReading(int* X, int* Y)
{
	*X = analogRead(joystickXADC);
	*Y = analogRead(joystickYADC);
}

int sign(int n)
{
	if(n > 0)
	{
		return 1;
	}
	return -1;
}

/* Get joystick position and turn it into motor velocities */
//
//   +y
//  -x +x
//    -y
//
void joystickSetMotors()
{
	int xAxis = 0;
	int yAxis = 0;

	const int XMiddle = 1023 / 2;
	const int YMiddle = 1023 / 2;

	getJoystickReading(&xAxis, &yAxis);

	//shift 0
	xAxis -= XMiddle;
	yAxis -= YMiddle;

	//make x go the right dir
	//xAxis *= -1;

	/* Apply deadzones */
	if (abs(xAxis) < DEAD_ZONE)
	{
		xAxis = 0;
	}
	if (abs(yAxis) < DEAD_ZONE)
	{
		yAxis = 0;
	}

	/* Calculate drive outputs -- magic*/
	//int leftVelocity = .5*float(yAxis) + 2 * sign(xAxis) * sqrt(abs(xAxis));
	//int rightVelocity = .5*float(yAxis) - 2 * sign(xAxis) * sqrt(abs(xAxis));
	//int leftVelocity = yAxis + 3 * sign(xAxis) * sqrt(abs(xAxis));
	//int rightVelocity = yAxis - 3 * sign(xAxis) * sqrt(abs(xAxis));
	int leftVelocity;
	int rightVelocity;

	if ((yAxis > xAxis)&&(xAxis >= 0)) {
	  leftVelocity = 512;
	  rightVelocity = 512 - xAxis/2;
	}
	else if ((yAxis <= xAxis)&&(yAxis > 0)) {
	  leftVelocity = 256 + yAxis/2;
	  rightVelocity = yAxis - 256;
	}
	else if ((-yAxis < xAxis)&&(yAxis <= 0)) {
	  leftVelocity = 256 - yAxis;
	  rightVelocity = -256 - yAxis/2;
	}
	else if ((-yAxis >= xAxis)&&(xAxis > 0)) {
	  leftVelocity = -512 + xAxis/2;
	  rightVelocity = -512;
	}
	else if ((yAxis < xAxis)&&(xAxis <= 0)) {
	  leftVelocity = -512;
	  rightVelocity = -512 - xAxis/2;
	}
	else if ((yAxis >= xAxis)&&(yAxis < 0)) {
	  leftVelocity = -256 + yAxis/2;
	  rightVelocity = -256 + yAxis;
	}
	else if ((-yAxis < xAxis)&&(yAxis >= 0)) {
	  leftVelocity = -256 + yAxis;
	  rightVelocity = 256 + yAxis/2;
	}
	else if ((-yAxis >= xAxis)&&(xAxis < 0)) {
	  leftVelocity = 512 + xAxis/2;
	  rightVelocity = 512;
	}

	leftVelocity = float(leftVelocity/2)*abs(float(xAxis + yAxis)/float(1023 - abs(xAxis - yAxis)));
	rightVelocity = float(rightVelocity/2)*abs(float(xAxis + yAxis)/float(1023 - abs(xAxis - yAxis)));

	/* Limit drive velcocities to vaild values and convert them to drive speeds and directions */

	Serial.print("L/Rev: ");
	Serial.println(leftVelocity, DEC);

	Serial.print("R/Rev: ");
	Serial.println(rightVelocity, DEC);

	byte leftSpeed = min(abs(leftVelocity), 255);
	byte leftDir = (leftVelocity < 0) ? MC_MOTOR_REVERSE : MC_MOTOR_FORWARD;
	byte rightSpeed = min(abs(rightVelocity), 255);
	byte rightDir = (rightVelocity < 0) ? MC_MOTOR_REVERSE : MC_MOTOR_FORWARD;

	setLeftMotorDutyCycle(leftDir, leftSpeed);
	setRightMotorDutyCycle(rightDir, rightSpeed);

}
<|endoftext|>


<commit_before>/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
/*=========================================================================
 *
 *  Portions of this file are subject to the VTK Toolkit Version 3 copyright.
 *
 *  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
 *
 *  For complete copyright, license and disclaimer of warranty information
 *  please refer to the NOTICE file at the top of the ITK source tree.
 *
 *=========================================================================*/
#ifndef itkTestingStretchIntensityImageFilter_hxx
#define itkTestingStretchIntensityImageFilter_hxx

#include "itkTestingStretchIntensityImageFilter.h"
#include "itkImageRegionIterator.h"
#include "itkImageRegionConstIterator.h"
#include "itkMath.h"

namespace itk
{
namespace Testing
{

template <typename TInputImage, typename TOutputImage>
StretchIntensityImageFilter<TInputImage, TOutputImage>::StretchIntensityImageFilter()
  : m_Scale(1.0)
  , m_Shift(0.0)
  , m_InputMinimum(NumericTraits<InputPixelType>::max())
  , m_InputMaximum(NumericTraits<InputPixelType>::ZeroValue())
  , m_OutputMinimum(NumericTraits<OutputPixelType>::NonpositiveMin())
  , m_OutputMaximum(NumericTraits<OutputPixelType>::max())
{
  this->DynamicMultiThreadingOn();
}

template <typename TInputImage, typename TOutputImage>
void
StretchIntensityImageFilter<TInputImage, TOutputImage>::BeforeThreadedGenerateData()
{
  if (m_OutputMinimum > m_OutputMaximum)
  {
    itkExceptionMacro(<< "Minimum output value cannot be greater than Maximum output value.");
    return;
  }

  const TInputImage * inputImage = this->GetInput();

  ImageRegionConstIteratorWithIndex<TInputImage> it(inputImage, inputImage->GetBufferedRegion());

  m_InputMaximum = NumericTraits<InputPixelType>::NonpositiveMin();
  m_InputMinimum = NumericTraits<InputPixelType>::max();

  while (!it.IsAtEnd())
  {
    const InputPixelType value = it.Get();
    if (value > m_InputMaximum)
    {
      m_InputMaximum = value;
    }
    if (value < m_InputMinimum)
    {
      m_InputMinimum = value;
    }
    ++it;
  }

  if (itk::Math::abs(m_InputMaximum - m_InputMinimum) > itk::Math::abs(NumericTraits<InputPixelType>::epsilon()))
  {
    m_Scale = (static_cast<RealType>(m_OutputMaximum) - static_cast<RealType>(m_OutputMinimum)) /
              (static_cast<RealType>(m_InputMaximum) - static_cast<RealType>(m_InputMinimum));
  }
  else if (m_InputMaximum > NumericTraits<InputPixelType>::epsilon())
  {
    m_Scale = (static_cast<RealType>(m_OutputMaximum) - static_cast<RealType>(m_OutputMinimum)) /
              static_cast<RealType>(m_InputMaximum);
  }
  else
  {
    m_Scale = 0.0;
  }

  m_Shift = static_cast<RealType>(m_OutputMinimum) - static_cast<RealType>(m_InputMinimum) * m_Scale;
}

template <typename TInputImage, typename TOutputImage>
void
StretchIntensityImageFilter<TInputImage, TOutputImage>::DynamicThreadedGenerateData(
  const OutputImageRegionType & outputRegionForThread)
{
  const TInputImage * inputPtr = this->GetInput();
  TOutputImage *      outputPtr = this->GetOutput(0);

  InputImageRegionType inputRegionForThread = outputRegionForThread;

  ImageRegionConstIterator<TInputImage> inputIt(inputPtr, inputRegionForThread);
  ImageRegionIterator<TOutputImage>     outputIt(outputPtr, outputRegionForThread);

  inputIt.GoToBegin();
  outputIt.GoToBegin();

  while (!inputIt.IsAtEnd())
  {
    const InputPixelType x = inputIt.Get();
    const RealType       value = static_cast<RealType>(x) * m_Scale + m_Shift;

    auto result = Math::Round<OutputPixelType>(value);

    result = (result > m_OutputMaximum) ? m_OutputMaximum : result;
    result = (result < m_OutputMinimum) ? m_OutputMinimum : result;

    outputIt.Set(result);

    ++inputIt;
    ++outputIt;
  }
}

template <typename TInputImage, typename TOutputImage>
void
StretchIntensityImageFilter<TInputImage, TOutputImage>::SetInput(const TInputImage * input)
{
  // Process object is not const-correct so the const_cast is required here
  this->ProcessObject::SetNthInput(0, const_cast<TInputImage *>(input));
}

template <typename TInputImage, typename TOutputImage>
const TInputImage *
StretchIntensityImageFilter<TInputImage, TOutputImage>::GetInput() const
{
  return itkDynamicCastInDebugMode<const TInputImage *>(this->GetPrimaryInput());
}

template <typename TInputImage, typename TOutputImage>
void
StretchIntensityImageFilter<TInputImage, TOutputImage>::PrintSelf(std::ostream & os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);

  os << indent << "Scale: " << static_cast<typename NumericTraits<RealType>::PrintType>(m_Scale) << std::endl;
  os << indent << "Shift: " << static_cast<typename NumericTraits<RealType>::PrintType>(m_Shift) << std::endl;

  os << indent << "Input Minimum: " << static_cast<typename NumericTraits<InputPixelType>::PrintType>(m_InputMinimum)
     << std::endl;
  os << indent << "Input Maximum: " << static_cast<typename NumericTraits<InputPixelType>::PrintType>(m_InputMaximum)
     << std::endl;
  os << indent << "Output Minimum: " << static_cast<typename NumericTraits<OutputPixelType>::PrintType>(m_OutputMinimum)
     << std::endl;
  os << indent << "Output Maximum: " << static_cast<typename NumericTraits<OutputPixelType>::PrintType>(m_OutputMaximum)
     << std::endl;
}
} // end namespace Testing
} // end namespace itk

#endif
<commit_msg>PERF: Use ImageBufferRange in Testing::StretchIntensityImageFilter<commit_after>/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
/*=========================================================================
 *
 *  Portions of this file are subject to the VTK Toolkit Version 3 copyright.
 *
 *  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
 *
 *  For complete copyright, license and disclaimer of warranty information
 *  please refer to the NOTICE file at the top of the ITK source tree.
 *
 *=========================================================================*/
#ifndef itkTestingStretchIntensityImageFilter_hxx
#define itkTestingStretchIntensityImageFilter_hxx

#include "itkTestingStretchIntensityImageFilter.h"
#include "itkImageBufferRange.h"
#include "itkImageRegionIterator.h"
#include "itkImageRegionConstIterator.h"
#include "itkMath.h"

namespace itk
{
namespace Testing
{

template <typename TInputImage, typename TOutputImage>
StretchIntensityImageFilter<TInputImage, TOutputImage>::StretchIntensityImageFilter()
  : m_Scale(1.0)
  , m_Shift(0.0)
  , m_InputMinimum(NumericTraits<InputPixelType>::max())
  , m_InputMaximum(NumericTraits<InputPixelType>::ZeroValue())
  , m_OutputMinimum(NumericTraits<OutputPixelType>::NonpositiveMin())
  , m_OutputMaximum(NumericTraits<OutputPixelType>::max())
{
  this->DynamicMultiThreadingOn();
}

template <typename TInputImage, typename TOutputImage>
void
StretchIntensityImageFilter<TInputImage, TOutputImage>::BeforeThreadedGenerateData()
{
  if (m_OutputMinimum > m_OutputMaximum)
  {
    itkExceptionMacro(<< "Minimum output value cannot be greater than Maximum output value.");
    return;
  }

  const TInputImage * inputImage = this->GetInput();
  m_InputMaximum = NumericTraits<InputPixelType>::NonpositiveMin();
  m_InputMinimum = NumericTraits<InputPixelType>::max();

  for (const InputPixelType value : Experimental::MakeImageBufferRange(inputImage))
  {
    if (value > m_InputMaximum)
    {
      m_InputMaximum = value;
    }
    if (value < m_InputMinimum)
    {
      m_InputMinimum = value;
    }
  }

  if (itk::Math::abs(m_InputMaximum - m_InputMinimum) > itk::Math::abs(NumericTraits<InputPixelType>::epsilon()))
  {
    m_Scale = (static_cast<RealType>(m_OutputMaximum) - static_cast<RealType>(m_OutputMinimum)) /
              (static_cast<RealType>(m_InputMaximum) - static_cast<RealType>(m_InputMinimum));
  }
  else if (m_InputMaximum > NumericTraits<InputPixelType>::epsilon())
  {
    m_Scale = (static_cast<RealType>(m_OutputMaximum) - static_cast<RealType>(m_OutputMinimum)) /
              static_cast<RealType>(m_InputMaximum);
  }
  else
  {
    m_Scale = 0.0;
  }

  m_Shift = static_cast<RealType>(m_OutputMinimum) - static_cast<RealType>(m_InputMinimum) * m_Scale;
}

template <typename TInputImage, typename TOutputImage>
void
StretchIntensityImageFilter<TInputImage, TOutputImage>::DynamicThreadedGenerateData(
  const OutputImageRegionType & outputRegionForThread)
{
  const TInputImage * inputPtr = this->GetInput();
  TOutputImage *      outputPtr = this->GetOutput(0);

  InputImageRegionType inputRegionForThread = outputRegionForThread;

  ImageRegionConstIterator<TInputImage> inputIt(inputPtr, inputRegionForThread);
  ImageRegionIterator<TOutputImage>     outputIt(outputPtr, outputRegionForThread);

  inputIt.GoToBegin();
  outputIt.GoToBegin();

  while (!inputIt.IsAtEnd())
  {
    const InputPixelType x = inputIt.Get();
    const RealType       value = static_cast<RealType>(x) * m_Scale + m_Shift;

    auto result = Math::Round<OutputPixelType>(value);

    result = (result > m_OutputMaximum) ? m_OutputMaximum : result;
    result = (result < m_OutputMinimum) ? m_OutputMinimum : result;

    outputIt.Set(result);

    ++inputIt;
    ++outputIt;
  }
}

template <typename TInputImage, typename TOutputImage>
void
StretchIntensityImageFilter<TInputImage, TOutputImage>::SetInput(const TInputImage * input)
{
  // Process object is not const-correct so the const_cast is required here
  this->ProcessObject::SetNthInput(0, const_cast<TInputImage *>(input));
}

template <typename TInputImage, typename TOutputImage>
const TInputImage *
StretchIntensityImageFilter<TInputImage, TOutputImage>::GetInput() const
{
  return itkDynamicCastInDebugMode<const TInputImage *>(this->GetPrimaryInput());
}

template <typename TInputImage, typename TOutputImage>
void
StretchIntensityImageFilter<TInputImage, TOutputImage>::PrintSelf(std::ostream & os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);

  os << indent << "Scale: " << static_cast<typename NumericTraits<RealType>::PrintType>(m_Scale) << std::endl;
  os << indent << "Shift: " << static_cast<typename NumericTraits<RealType>::PrintType>(m_Shift) << std::endl;

  os << indent << "Input Minimum: " << static_cast<typename NumericTraits<InputPixelType>::PrintType>(m_InputMinimum)
     << std::endl;
  os << indent << "Input Maximum: " << static_cast<typename NumericTraits<InputPixelType>::PrintType>(m_InputMaximum)
     << std::endl;
  os << indent << "Output Minimum: " << static_cast<typename NumericTraits<OutputPixelType>::PrintType>(m_OutputMinimum)
     << std::endl;
  os << indent << "Output Maximum: " << static_cast<typename NumericTraits<OutputPixelType>::PrintType>(m_OutputMaximum)
     << std::endl;
}
} // end namespace Testing
} // end namespace itk

#endif
<|endoftext|>


<commit_before>AliAnalysisTaskJetHBOM *AddTaskJetHBOM(char* bRec = "AOD",char* bGen = "",UInt_t filterMask = 272, UInt_t iPhysicsSelectionFlag = AliVEvent::kAny,Char_t *jf = "KT", Float_t radius = 0.4,Int_t kWriteAOD = kFALSE,char* deltaFile = "",Float_t ptTrackCut = 0.15, Float_t etaTrackWindow = 0.9,Float_t vertexWindow = 10.,TString effLoc = "$ALICE_ROOT/OADB/PWGJE/HBOM/fastMCInput_LHC10h_110719a.root",Int_t fNHBOM = 0);

Int_t kBackgroundModeCl = 0;
Float_t kPtTrackCutCl = 0.15;
Float_t kTrackEtaWindowCl = 0.8;
Float_t kVertexWindowCl = 10;

AliAnalysisTaskJetHBOM *AddTaskJetHBOM(char* bRec,char* bGen ,UInt_t filterMask,UInt_t iPhysicsSelectionFlag,Char_t *jf,Float_t radius,Int_t kWriteAOD,char *deltaFile,Float_t ptTrackCut,Float_t etaTrackWindow,Float_t vertexWindow,TString effLoc,Int_t fNHBOM)
 {
 // Creates a jet fider task, configures it and adds it to the analysis manager.
   kPtTrackCutCl = ptTrackCut;
   kTrackEtaWindowCl = etaTrackWindow;
   kVertexWindowCl = vertexWindow;

   TString outputFile(deltaFile);
    // Get the pointer to the existing analysis manager via the static access method.
    //==============================================================================
    AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
    if (!mgr) {
       ::Error("AddTaskJetHBOM", "No analysis manager to connect to.");
       return NULL;
    }

    // Check the analysis type using the event handlers connected to the analysis manager.
    //==============================================================================
    if (!mgr->GetInputEventHandler()) {
      ::Error("AddTaskJetHBOM", "This task requires an input event handler");
       return NULL;
    }

    TString type = mgr->GetInputEventHandler()->GetDataType();
    TString typeRec(bRec);
    TString typeGen(bGen);
    if(!typeRec.Contains("AODextra")) {
      typeGen.ToUpper();
      typeRec.ToUpper();
    }
    cout << "typeRec: " << typeRec << endl;
    // Create the task and configure it.
    //===========================================================================

    TString cAdd = "";
    cAdd += Form("%02d_",(int)((radius+0.01)*10.));
    cAdd += Form("B%d",(int)kBackgroundModeCl);
    cAdd += Form("_Filter%05d",filterMask);
    cAdd += Form("_Cut%05d",(int)(1000.*kPtTrackCutCl));
    cAdd += Form("_hbom%02d",fNHBOM);
    Printf("%s %E",cAdd.Data(),kPtTrackCutCl);
    AliAnalysisTaskJetHBOM* hbom = new  AliAnalysisTaskJetHBOM(Form("JetHBOM%s_%s%s",bRec,jf,cAdd.Data()));

   hbom->SetFilterMask(filterMask);
   //   hbom->SetUseGlobalSelection(kTRUE);
   hbom->SetVtxCuts(kVertexWindowCl,1);//sets fVtxZCut and fVtxR2Cut
   if(type == "AOD"){
     // Assume all jet are produced already
     hbom->SetAODTrackInput(kTRUE);
     hbom->SetAODMCInput(kTRUE);
   }

   if(typeRec.Contains("AODMC2b")){// work down from the top AODMC2b -> AODMC2 -> AODMC -> AOD
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODMCChargedAcceptance);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(kTrackEtaWindowCl);
   }
   else if (typeRec.Contains("AODMC2")){
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODMCCharged);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(5);
   }
   else if (typeRec.Contains("AODMC")){
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODMCAll);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(5);
   }
   else if (typeRec.Contains("AODextraonly")) {
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODextraonly);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(kTrackEtaWindowCl);
   }
   else if (typeRec.Contains("AODextra")) {
     cout << "AliAnalysisTaskJetHBOM::kTrackAODextra: " << AliAnalysisTaskJetHBOM::kTrackAODextra << endl;
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODextra);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(kTrackEtaWindowCl);
   }
   else if (typeRec.Contains("AOD")) {
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAOD);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(kTrackEtaWindowCl);
   }

   hbom->SetRparam(radius);
   hbom->SetGhostArea(0.005);
   hbom->SetGhostEtamax(kTrackEtaWindowCl);

   switch (jf) {
   case "ANTIKT":
     hbom->SetAlgorithm(2); // antikt from fastjet/JetDefinition.hh
     break;
   case "CA":
     hbom->SetAlgorithm(1); // CA from fastjet/JetDefinition.hh
     break;
   case "KT":
     hbom->SetAlgorithm(0); // kt from fastjet/JetDefinition.hh
     break;
   default:
     ::Error("AddTaskJetHBOM", "Wrong jet finder selected\n");
     return 0;
   }

   if(kWriteAOD){
     if(outputFile.Length())hbom->SetJetOutputFile(outputFile);
     hbom->SetJetOutputBranch(Form("hbom%s_%s%s",bRec,jf,cAdd.Data()));
     hbom->SetJetOutputMinPt(0); // store only jets above a certain threshold
   }

   //sets number of detector hits
   hbom->SetfNHBOM(fNHBOM);

   //physics Selection
   if(iPhysicsSelectionFlag)hbom->SelectCollisionCandidates(iPhysicsSelectionFlag);

   mgr->AddTask(hbom);

   // Create ONLY the output containers for the data produced by the task.
   // Get and connect other common input/output containers via the manager as below
   //==============================================================================
   AliAnalysisDataContainer *coutput1_Spec = mgr->CreateContainer(Form("hbom_%s_%s_%s%s",bRec,bGen,jf,cAdd.Data()), TList::Class(),AliAnalysisManager::kOutputContainer,Form("%s:PWGJE_hbom_%s_%s_%s%s",AliAnalysisManager::GetCommonFileName(),bRec,bGen,jf,cAdd.Data()));

   mgr->ConnectInput  (hbom, 0, mgr->GetCommonInputContainer());
   mgr->ConnectOutput (hbom, 0, mgr->GetCommonOutputContainer());
   mgr->ConnectOutput (hbom,  1, coutput1_Spec );

   //loads efficiencies
   hbom->SetEfficiencyPt(GetEfficiencyPt(effLoc));
   hbom->SetEfficiencyPhi(GetEfficiencyPhi(effLoc));

   return hbom;
}

//loads single track pT efficiency from root file
TH1F *GetEfficiencyPt(TString effLoc){
  static TFile *fIn = 0;
  static TH1F *hEffPt = 0;

  if(!fIn)fIn = TFile::Open(effLoc.Data());
  if(!hEffPt)hEffPt = (TH1F*)fIn->Get("hSingleTrackEffPt");
  if(!hEffPt) cout<<"Could not load hSingleTrackEffPt"<<endl;

  if(hEffPt){return hEffPt;}
  return 0;

}

//loads phi-pT efficiency from root file
TH2D *GetEfficiencyPhi(TString effLoc){
  static TFile *fIn = 0;
  static TH2D *hPhiPt = 0;

  if(!fIn)fIn = TFile::Open(effLoc.Data());
  if(!hPhiPt)hPhiPt = (TH2D*)fIn->Get("h2TrackPtPhiNorm");
  if(!hPhiPt) cout<<"Could not load h2TrackPtPhiNorm"<<endl;

  if(hPhiPt){return hPhiPt;}
  return 0;

}
<commit_msg>Fix name of output container (M. Zimmermann)<commit_after>AliAnalysisTaskJetHBOM *AddTaskJetHBOM(char* bRec = "AOD",char* bGen = "",UInt_t filterMask = 272, UInt_t iPhysicsSelectionFlag = AliVEvent::kAny,Char_t *jf = "KT", Float_t radius = 0.4,Int_t kWriteAOD = kFALSE,char* deltaFile = "",Float_t ptTrackCut = 0.15, Float_t etaTrackWindow = 0.9,Float_t vertexWindow = 10.,TString effLoc = "$ALICE_ROOT/OADB/PWGJE/HBOM/fastMCInput_LHC10h_110719a.root",Int_t fNHBOM = 0, Int_t constCone = kFALSE, Float_t constConePhi = 0, Float_t constConeEta = 0);

Int_t kBackgroundModeCl = 0;
Float_t kPtTrackCutCl = 0.15;
Float_t kTrackEtaWindowCl = 0.8;
Float_t kVertexWindowCl = 10;

AliAnalysisTaskJetHBOM *AddTaskJetHBOM(char* bRec,char* bGen ,UInt_t filterMask,UInt_t iPhysicsSelectionFlag,Char_t *jf,Float_t radius,Int_t kWriteAOD,char *deltaFile,Float_t ptTrackCut,Float_t etaTrackWindow,Float_t vertexWindow,TString effLoc,Int_t fNHBOM, Int_t constCone, Float_t constConePhi,Float_t constConeEta)
 {
   //if constCone is true, the random Cone positon is set to constConePhi and constConeEta. Else the cone is random set and the parameters constConePhi and constConeEta are irrelevant

 // Creates a jet fider task, configures it and adds it to the analysis manager.
   kPtTrackCutCl = ptTrackCut;
   kTrackEtaWindowCl = etaTrackWindow;
   kVertexWindowCl = vertexWindow;

   TString outputFile(deltaFile);
    // Get the pointer to the existing analysis manager via the static access method.
    //==============================================================================
    AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
    if (!mgr) {
       ::Error("AddTaskJetHBOM", "No analysis manager to connect to.");
       return NULL;
    }

    // Check the analysis type using the event handlers connected to the analysis manager.
    //==============================================================================
    if (!mgr->GetInputEventHandler()) {
      ::Error("AddTaskJetHBOM", "This task requires an input event handler");
       return NULL;
    }

    TString type = mgr->GetInputEventHandler()->GetDataType();
    TString typeRec(bRec);
    TString typeGen(bGen);
    if(!typeRec.Contains("AODextra")) {
      typeGen.ToUpper();
      typeRec.ToUpper();
    }
    cout << "typeRec: " << typeRec << endl;
    // Create the task and configure it.
    //===========================================================================

    TString cAdd = "";
    cAdd += Form("%02d_",(int)((radius+0.01)*10.));
    cAdd += Form("B%d",(int)kBackgroundModeCl);
    cAdd += Form("_Filter%05d",filterMask);
    cAdd += Form("_Cut%05d",(int)(1000.*kPtTrackCutCl));
    cAdd += Form("_hbom%02d",fNHBOM);
    if(constCone){
      cAdd += Form("_constConePhi%02dEta%02d",constConePhi,constConeEta);
    }
    Printf("%s %E",cAdd.Data(),kPtTrackCutCl);
    AliAnalysisTaskJetHBOM* hbom = new  AliAnalysisTaskJetHBOM(Form("JetHBOM%s_%s%s",bRec,jf,cAdd.Data()));

   hbom->SetFilterMask(filterMask);
   //   hbom->SetUseGlobalSelection(kTRUE);
   hbom->SetVtxCuts(kVertexWindowCl,1);//sets fVtxZCut and fVtxR2Cut
   if(type == "AOD"){
     // Assume all jet are produced already
     hbom->SetAODTrackInput(kTRUE);
     hbom->SetAODMCInput(kTRUE);
   }

   if(typeRec.Contains("AODMC2b")){// work down from the top AODMC2b -> AODMC2 -> AODMC -> AOD
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODMCChargedAcceptance);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(kTrackEtaWindowCl);
   }
   else if (typeRec.Contains("AODMC2")){
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODMCCharged);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(5);
   }
   else if (typeRec.Contains("AODMC")){
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODMCAll);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(5);
   }
   else if (typeRec.Contains("AODextraonly")) {
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODextraonly);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(kTrackEtaWindowCl);
   }
   else if (typeRec.Contains("AODextra")) {
     cout << "AliAnalysisTaskJetHBOM::kTrackAODextra: " << AliAnalysisTaskJetHBOM::kTrackAODextra << endl;
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAODextra);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(kTrackEtaWindowCl);
   }
   else if (typeRec.Contains("AOD")) {
     hbom->SetTrackTypeRec(AliAnalysisTaskJetHBOM::kTrackAOD);
     hbom->SetTrackPtCut(kPtTrackCutCl);
     hbom->SetTrackEtaWindow(kTrackEtaWindowCl);
   }

   hbom->SetRparam(radius);
   hbom->SetGhostArea(0.005);
   hbom->SetGhostEtamax(kTrackEtaWindowCl);

   switch (jf) {
   case "ANTIKT":
     hbom->SetAlgorithm(2); // antikt from fastjet/JetDefinition.hh
     break;
   case "CA":
     hbom->SetAlgorithm(1); // CA from fastjet/JetDefinition.hh
     break;
   case "KT":
     hbom->SetAlgorithm(0); // kt from fastjet/JetDefinition.hh
     break;
   default:
     ::Error("AddTaskJetHBOM", "Wrong jet finder selected\n");
     return 0;
   }

   //Constant Cone analysis
   if(constCone){
     hbom->SetRandConePos(constConeEta,constConePhi);
   }

   if(kWriteAOD){
     if(outputFile.Length())hbom->SetJetOutputFile(outputFile);
     hbom->SetJetOutputBranch(Form("hbom%s_%s%s",bRec,jf,cAdd.Data()));
     hbom->SetJetOutputMinPt(0); // store only jets above a certain threshold
   }

   //sets number of detector hits
   hbom->SetfNHBOM(fNHBOM);

   //physics Selection
   if(iPhysicsSelectionFlag)hbom->SelectCollisionCandidates(iPhysicsSelectionFlag);

   mgr->AddTask(hbom);

   // Create ONLY the output containers for the data produced by the task.
   // Get and connect other common input/output containers via the manager as below
   //==============================================================================
   AliAnalysisDataContainer *coutput1_Spec = mgr->CreateContainer(Form("hbom_%s_%s_%s%s",bRec,bGen,jf,cAdd.Data()), TList::Class(),AliAnalysisManager::kOutputContainer,Form("%s:PWGJE_hbom_%s_%s_%s%s",AliAnalysisManager::GetCommonFileName(),bRec,bGen,jf,cAdd.Data()));

   mgr->ConnectInput  (hbom, 0, mgr->GetCommonInputContainer());
   mgr->ConnectOutput (hbom, 0, mgr->GetCommonOutputContainer());
   mgr->ConnectOutput (hbom,  1, coutput1_Spec );

   //loads efficiencies
   hbom->SetEfficiencyPt(GetEfficiencyPt(effLoc));
   hbom->SetEfficiencyPhi(GetEfficiencyPhi(effLoc));

   return hbom;
}

//loads single track pT efficiency from root file
TH1F *GetEfficiencyPt(TString effLoc){
  static TFile *fIn = 0;
  static TH1F *hEffPt = 0;

  if(!fIn)fIn = TFile::Open(effLoc.Data());
  if(!hEffPt)hEffPt = (TH1F*)fIn->Get("hSingleTrackEffPt");
  if(!hEffPt) cout<<"Could not load hSingleTrackEffPt"<<endl;

  if(hEffPt){return hEffPt;}
  return 0;

}

//loads phi-pT efficiency from root file
TH2D *GetEfficiencyPhi(TString effLoc){
  static TFile *fIn = 0;
  static TH2D *hPhiPt = 0;

  if(!fIn)fIn = TFile::Open(effLoc.Data());
  if(!hPhiPt)hPhiPt = (TH2D*)fIn->Get("h2TrackPtPhiNorm");
  if(!hPhiPt) cout<<"Could not load h2TrackPtPhiNorm"<<endl;

  if(hPhiPt){return hPhiPt;}
  return 0;

}
<|endoftext|>


<commit_before>/**
 * @file PathPlanner.cpp
 *
 * @author Yigit Can Akcay
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"

PathPlanner::PathPlanner()
:
step_buffer({}),
foot_to_use(Foot::RIGHT),
last_stepRequestID(getMotionStatus().stepControl.stepRequestID + 1),      // WalkRequest stepRequestID starts at 0, we have to start at 1
kick_planned(false)
{
  DEBUG_REQUEST_REGISTER("PathPlanner:walk_to_ball", "Walks to the ball from far.", false);

  getDebugParameterList().add(&params);
}

PathPlanner::~PathPlanner()
{
  getDebugParameterList().remove(&params);
}

void PathPlanner::execute()
{
  // --- DEBUG REQUESTS ---
  DEBUG_REQUEST("PathPlanner:walk_to_ball",
                if (getBallModel().positionPreview.x > 250)
                {
                  walk_to_ball(Foot::NONE);
                }
                );
  // --- DEBUG REQUESTS ---

  getPathModel().kick_executed = false;

  STOPWATCH_START("PathPlanner");
  // Always executed first
  manage_step_buffer();

  // The kick has been executed
  // Tells XABSL to jump into next state
  if (kick_planned && step_buffer.empty()) {
    getPathModel().kick_executed = true;
  }

  switch (getPathModel().path_routine)
  {
  case PathModel::PathRoutine::NONE:
    // There is no kick planned, since the kick has been executed
    // and XABSL is in a different state now
    if (kick_planned) {
      kick_planned = false;
    }

    if (step_buffer.empty()) {
      return;
    }
    break;
  case PathModel::PathRoutine::GO_TO_BALL_FAST:
    walk_to_ball(Foot::NONE , true);
    break;
  case PathModel::PathRoutine::GO_TO_BALL_SLOW:
    walk_to_ball(Foot::NONE);
    break;
  case PathModel::PathRoutine::MOVE_AROUND_BALL:
    move_around_ball(getPathModel().direction, getPathModel().radius);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_LEFT:
    approach_ball(Foot::LEFT);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_RIGHT:
    approach_ball(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_LEFT:
    short_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_RIGHT:
    short_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::LONG_KICK_LEFT:
    long_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::LONG_KICK_RIGHT:
    long_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SIDEKICK_LEFT:
    sidekick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SIDEKICK_RIGHT:
    sidekick(Foot::RIGHT);
    break;
  }

  // Always executed last
  execute_step_buffer();

  STOPWATCH_STOP("PathPlanner");
}

void PathPlanner::walk_to_ball(const Foot foot, const bool go_fast)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ballPos    = getBallModel().positionPreview;
      coordinate = WalkRequest::Hip;
      break;
  }
  double ballRotation = ballPos.angle();

  Pose2D pose           = { ballRotation, 0.7*(ballPos.x - getPathModel().distance), ballPos.y };

  if (step_buffer.empty())
  {
    StepType type         = StepType::WALKSTEP;
    double scale          = 1.0;
    double speed_direction = 0.0;

    if (go_fast)
    {
      double character = 1.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
    else
    {
      double character = 0.3;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::move_around_ball(const double direction, const double radius)
{
  Vector2d ballPos                   = getBallModel().positionPreview;
  double ballRotation                = ballPos.angle();
  double ballDistance                = ballPos.abs();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;

  double min1;
  double min2;
  double max1;
  double max2;
  if (direction <= 0)
  {
    min1 = 0.0;
    min2 = 0.0;
    max1 = 45.0;
    max2 = 100.0;
  }
  else {
    min1 = -45;
    min2 = -100;
    max1 = 0;
    max2 = 0;
  }

  double stepX = (ballDistance - radius) * std::cos(ballRotation);
  double stepY = Math::clamp(radius * std::tan(Math::clamp(Math::toDegrees(-direction), min1, max1)), min2, max2) * std::cos(ballRotation);

  Pose2D pose           = { ballRotation, stepX, stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    Foot foot              = Foot::NONE;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::approach_ball(const Foot foot)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  double stepX                       = 0.0;
  double stepY                       = 0.0;
  double ballRadius                  = getFieldInfo().ballRadius;
  double stepRotation                = ballPos.abs() > 250 ? ballPos.angle() : 0;

  switch (foot) {
  case Foot::LEFT:
    ballPos    = getBallModel().positionPreviewInLFoot;
    coordinate = WalkRequest::LFoot;
    stepX      = ballPos.x - std::abs(ballPos.y - getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y - getPathModel().yOffset;
    break;
  case Foot::RIGHT:
    ballPos    = getBallModel().positionPreviewInRFoot;
    coordinate = WalkRequest::RFoot;
    stepX      = ballPos.x - std::abs(ballPos.y + getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y + getPathModel().yOffset;
    break;
  case Foot::NONE:
    ASSERT(false);
  }

  Pose2D pose;
  if (   params.approach_ball_adapt_control
      && Pose2D(0.7*stepX, 0.7*stepY).translation.abs() < params.approach_ball_adapt_threshold)
  {
    pose = { stepRotation, stepX, stepY };
  }
  else
  {
    pose = { stepRotation, 0.7 * stepX, 0.7 * stepY };
  }

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, Foot::NONE, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::short_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500 , 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::long_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500, 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::sidekick(const Foot foot)
{
  if (!kick_planned)
  {
    double speed_direction             = 0.0;
    double stepY                       = 0.0;
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate     = WalkRequest::LFoot;
      speed_direction = 90;
      stepY          = 100;
      break;
    case Foot::RIGHT:
      coordinate     = WalkRequest::RFoot;
      speed_direction = -90;
      stepY          = -100;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, 500, stepY };
      StepType type = StepType::KICKSTEP;
      double character = 1.0;
      Foot step_foot = foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT;
      double scale = params.sidekick_scale;

      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

// Stepcontrol
void PathPlanner::add_step(Pose2D &pose, const StepType &type, const WalkRequest::Coordinate &coordinate, const double character, const Foot foot, const double scale, const double speedDirection, const WalkRequest::StepControlRequest::RestrictionMode restriction, bool isProtected)
{
  step_buffer.push_back(Step_Buffer_Element({ pose,
                                              speedDirection,
                                              type,
                                              type == StepType::KICKSTEP ? params.kick_time : 250,
                                              character,
                                              scale,
                                              foot,
                                              coordinate,
                                              restriction,
                                              isProtected}));
}
void PathPlanner::update_step(Pose2D &pose)
{
  ASSERT(step_buffer.size() > 0);

  step_buffer.front().pose = pose;
}

void PathPlanner::manage_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  // requested step has been accepted
  if (last_stepRequestID == getMotionStatus().stepControl.stepRequestID)
  {
    step_buffer.erase(step_buffer.begin());
    last_stepRequestID = getMotionStatus().stepControl.stepRequestID + 1;
  }
}

void PathPlanner::execute_step_buffer()
{
  STOPWATCH_START("PathPlanner:execute_steplist");

  if (step_buffer.empty()) {
    return;
  }

  getMotionRequest().id                                        = motion::walk;
  getMotionRequest().walkRequest.coordinate                    = step_buffer.front().coordinate;
  getMotionRequest().walkRequest.character                     = step_buffer.front().character;
  getMotionRequest().walkRequest.stepControl.scale             = step_buffer.front().scale;
  getMotionRequest().walkRequest.stepControl.stepID            = getMotionStatus().stepControl.stepID;
  getMotionRequest().walkRequest.stepControl.type              = step_buffer.front().type;
  getMotionRequest().walkRequest.stepControl.time              = step_buffer.front().time;
  getMotionRequest().walkRequest.stepControl.speedDirection    = step_buffer.front().speedDirection;
  getMotionRequest().walkRequest.stepControl.target            = step_buffer.front().pose;
  getMotionRequest().walkRequest.stepControl.restriction       = step_buffer.front().restriction;
  getMotionRequest().walkRequest.stepControl.isProtected       = step_buffer.front().isProtected;
  getMotionRequest().walkRequest.stepControl.stepRequestID     = last_stepRequestID;

  // normal walking WALKSTEPs use Foot::NONE, for KICKSTEPs the foot to use has to be specified
  if (step_buffer.front().foot == Foot::NONE)
  {
    switch (getMotionStatus().stepControl.moveableFoot)
    {
    case MotionStatus::StepControlStatus::LEFT:
      foot_to_use = Foot::LEFT;
      break;
    case MotionStatus::StepControlStatus::RIGHT:
      foot_to_use = Foot::RIGHT;
      break;
    case MotionStatus::StepControlStatus::BOTH:
      if (step_buffer.front().pose.translation.y > 0.0f || step_buffer.front().pose.rotation > 0.0f)
      {
        foot_to_use = Foot::LEFT;
      }
      else
      {
        foot_to_use = Foot::RIGHT;
      }
      break;
    case MotionStatus::StepControlStatus::NONE:
      foot_to_use = Foot::RIGHT;
      break;
    }
  }
  else
  {
    foot_to_use = step_buffer.front().foot;
  }
  // false means right foot
  getMotionRequest().walkRequest.stepControl.moveLeftFoot = (foot_to_use != Foot::RIGHT);
  STOPWATCH_STOP("PathPlanner:execute_steplist");
}
<commit_msg>more clear implementation<commit_after>/**
 * @file PathPlanner.cpp
 *
 * @author Yigit Can Akcay
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"

PathPlanner::PathPlanner()
:
step_buffer({}),
foot_to_use(Foot::RIGHT),
last_stepRequestID(getMotionStatus().stepControl.stepRequestID + 1),      // WalkRequest stepRequestID starts at 0, we have to start at 1
kick_planned(false)
{
  DEBUG_REQUEST_REGISTER("PathPlanner:walk_to_ball", "Walks to the ball from far.", false);

  getDebugParameterList().add(&params);
}

PathPlanner::~PathPlanner()
{
  getDebugParameterList().remove(&params);
}

void PathPlanner::execute()
{
  // --- DEBUG REQUESTS ---
  DEBUG_REQUEST("PathPlanner:walk_to_ball",
                if (getBallModel().positionPreview.x > 250)
                {
                  walk_to_ball(Foot::NONE);
                }
                );
  // --- DEBUG REQUESTS ---

  getPathModel().kick_executed = false;

  STOPWATCH_START("PathPlanner");
  // Always executed first
  manage_step_buffer();

  // The kick has been executed
  // Tells XABSL to jump into next state
  if (kick_planned && step_buffer.empty()) {
    getPathModel().kick_executed = true;
  }

  switch (getPathModel().path_routine)
  {
  case PathModel::PathRoutine::NONE:
    // There is no kick planned, since the kick has been executed
    // and XABSL is in a different state now
    if (kick_planned) {
      kick_planned = false;
    }

    if (step_buffer.empty()) {
      return;
    }
    break;
  case PathModel::PathRoutine::GO_TO_BALL_FAST:
    walk_to_ball(Foot::NONE , true);
    break;
  case PathModel::PathRoutine::GO_TO_BALL_SLOW:
    walk_to_ball(Foot::NONE);
    break;
  case PathModel::PathRoutine::MOVE_AROUND_BALL:
    move_around_ball(getPathModel().direction, getPathModel().radius);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_LEFT:
    approach_ball(Foot::LEFT);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_RIGHT:
    approach_ball(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_LEFT:
    short_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_RIGHT:
    short_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::LONG_KICK_LEFT:
    long_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::LONG_KICK_RIGHT:
    long_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SIDEKICK_LEFT:
    sidekick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SIDEKICK_RIGHT:
    sidekick(Foot::RIGHT);
    break;
  }

  // Always executed last
  execute_step_buffer();

  STOPWATCH_STOP("PathPlanner");
}

void PathPlanner::walk_to_ball(const Foot foot, const bool go_fast)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ballPos    = getBallModel().positionPreview;
      coordinate = WalkRequest::Hip;
      break;
  }
  double ballRotation = ballPos.angle();

  Pose2D pose           = { ballRotation, 0.7*(ballPos.x - getPathModel().distance), ballPos.y };

  if (step_buffer.empty())
  {
    StepType type         = StepType::WALKSTEP;
    double scale          = 1.0;
    double speed_direction = 0.0;

    if (go_fast)
    {
      double character = 1.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
    else
    {
      double character = 0.3;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::move_around_ball(const double direction, const double radius)
{
  Vector2d ballPos                   = getBallModel().positionPreview;
  double ballRotation                = ballPos.angle();
  double ballDistance                = ballPos.abs();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;

  double min1;
  double min2;
  double max1;
  double max2;
  if (direction <= 0)
  {
    min1 = 0.0;
    min2 = 0.0;
    max1 = 45.0;
    max2 = 100.0;
  }
  else {
    min1 = -45;
    min2 = -100;
    max1 = 0;
    max2 = 0;
  }

  double stepX = (ballDistance - radius) * std::cos(ballRotation);
  double stepY = Math::clamp(radius * std::tan(Math::clamp(Math::toDegrees(-direction), min1, max1)), min2, max2) * std::cos(ballRotation);

  Pose2D pose           = { ballRotation, stepX, stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    Foot foot              = Foot::NONE;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::approach_ball(const Foot foot)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  double stepX                       = 0.0;
  double stepY                       = 0.0;
  double ballRadius                  = getFieldInfo().ballRadius;
  double stepRotation                = ballPos.abs() > 250 ? ballPos.angle() : 0;

  switch (foot) {
  case Foot::LEFT:
    ballPos    = getBallModel().positionPreviewInLFoot;
    coordinate = WalkRequest::LFoot;
    stepX      = ballPos.x - std::abs(ballPos.y - getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y - getPathModel().yOffset;
    break;
  case Foot::RIGHT:
    ballPos    = getBallModel().positionPreviewInRFoot;
    coordinate = WalkRequest::RFoot;
    stepX      = ballPos.x - std::abs(ballPos.y + getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y + getPathModel().yOffset;
    break;
  case Foot::NONE:
    ASSERT(false);
  }

  const double slow_down_factor = 0.7;
  Pose2D pose;
  if (   params.approach_ball_adapt_control
      && Vector2d(stepX, stepY).abs() < params.approach_ball_adapt_threshold)
  {
    pose = { stepRotation, stepX, stepY };
  }
  else
  {
    pose = { stepRotation, slow_down_factor * stepX, slow_down_factor * stepY };
  }

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, Foot::NONE, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::short_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500 , 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::long_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500, 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::sidekick(const Foot foot)
{
  if (!kick_planned)
  {
    double speed_direction             = 0.0;
    double stepY                       = 0.0;
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate     = WalkRequest::LFoot;
      speed_direction = 90;
      stepY          = 100;
      break;
    case Foot::RIGHT:
      coordinate     = WalkRequest::RFoot;
      speed_direction = -90;
      stepY          = -100;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, 500, stepY };
      StepType type = StepType::KICKSTEP;
      double character = 1.0;
      Foot step_foot = foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT;
      double scale = params.sidekick_scale;

      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

// Stepcontrol
void PathPlanner::add_step(Pose2D &pose, const StepType &type, const WalkRequest::Coordinate &coordinate, const double character, const Foot foot, const double scale, const double speedDirection, const WalkRequest::StepControlRequest::RestrictionMode restriction, bool isProtected)
{
  step_buffer.push_back(Step_Buffer_Element({ pose,
                                              speedDirection,
                                              type,
                                              type == StepType::KICKSTEP ? params.kick_time : 250,
                                              character,
                                              scale,
                                              foot,
                                              coordinate,
                                              restriction,
                                              isProtected}));
}
void PathPlanner::update_step(Pose2D &pose)
{
  ASSERT(step_buffer.size() > 0);

  step_buffer.front().pose = pose;
}

void PathPlanner::manage_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  // requested step has been accepted
  if (last_stepRequestID == getMotionStatus().stepControl.stepRequestID)
  {
    step_buffer.erase(step_buffer.begin());
    last_stepRequestID = getMotionStatus().stepControl.stepRequestID + 1;
  }
}

void PathPlanner::execute_step_buffer()
{
  STOPWATCH_START("PathPlanner:execute_steplist");

  if (step_buffer.empty()) {
    return;
  }

  getMotionRequest().id                                        = motion::walk;
  getMotionRequest().walkRequest.coordinate                    = step_buffer.front().coordinate;
  getMotionRequest().walkRequest.character                     = step_buffer.front().character;
  getMotionRequest().walkRequest.stepControl.scale             = step_buffer.front().scale;
  getMotionRequest().walkRequest.stepControl.stepID            = getMotionStatus().stepControl.stepID;
  getMotionRequest().walkRequest.stepControl.type              = step_buffer.front().type;
  getMotionRequest().walkRequest.stepControl.time              = step_buffer.front().time;
  getMotionRequest().walkRequest.stepControl.speedDirection    = step_buffer.front().speedDirection;
  getMotionRequest().walkRequest.stepControl.target            = step_buffer.front().pose;
  getMotionRequest().walkRequest.stepControl.restriction       = step_buffer.front().restriction;
  getMotionRequest().walkRequest.stepControl.isProtected       = step_buffer.front().isProtected;
  getMotionRequest().walkRequest.stepControl.stepRequestID     = last_stepRequestID;

  // normal walking WALKSTEPs use Foot::NONE, for KICKSTEPs the foot to use has to be specified
  if (step_buffer.front().foot == Foot::NONE)
  {
    switch (getMotionStatus().stepControl.moveableFoot)
    {
    case MotionStatus::StepControlStatus::LEFT:
      foot_to_use = Foot::LEFT;
      break;
    case MotionStatus::StepControlStatus::RIGHT:
      foot_to_use = Foot::RIGHT;
      break;
    case MotionStatus::StepControlStatus::BOTH:
      if (step_buffer.front().pose.translation.y > 0.0f || step_buffer.front().pose.rotation > 0.0f)
      {
        foot_to_use = Foot::LEFT;
      }
      else
      {
        foot_to_use = Foot::RIGHT;
      }
      break;
    case MotionStatus::StepControlStatus::NONE:
      foot_to_use = Foot::RIGHT;
      break;
    }
  }
  else
  {
    foot_to_use = step_buffer.front().foot;
  }
  // false means right foot
  getMotionRequest().walkRequest.stepControl.moveLeftFoot = (foot_to_use != Foot::RIGHT);
  STOPWATCH_STOP("PathPlanner:execute_steplist");
}
<|endoftext|>


<commit_before>#include <iostream>
#include "TChain.h"
#include "TH1F.h"
#include "TList.h"
#include "TGeoGlobalMagField.h"
#include "AliAnalysisTask.h"
#include "AliAnalysisManager.h"
#include "AliESDEvent.h"
#include "AliESDInputHandler.h"
#include "AliESDtrack.h"
#include "AliAODEvent.h"
#include "AliHeader.h"
#include "AliMCEvent.h"
#include "AliGenEventHeader.h"
#include "AliESDtrackCuts.h"
#include "AlidNdPtTools.h"
#include "AliAnalysisTaskMKBase.h"
#include "AliAnalysisTaskDCArStudy.h"

class AliAnalysisTaskDCArStudy;

using namespace std;

ClassImp(AliAnalysisTaskDCArStudy)

//_____________________________________________________________________________

AliAnalysisTaskDCArStudy::AliAnalysisTaskDCArStudy()
    : AliAnalysisTaskMKBase()
    , fHistDCA(0)
    , fHistDCATPC(0)
{
    // default contructor

}

//_____________________________________________________________________________

AliAnalysisTaskDCArStudy::AliAnalysisTaskDCArStudy(const char* name)
    : AliAnalysisTaskMKBase(name)
    , fHistDCA(0)
    , fHistDCATPC(0)
{
    // constructor

}

//_____________________________________________________________________________

AliAnalysisTaskDCArStudy::~AliAnalysisTaskDCArStudy()
{
    // destructor
}

//_____________________________________________________________________________

void AliAnalysisTaskDCArStudy::AddOutput()
{
    //dcar:pt:mult:cent:mcinfo
    AddAxis("DCAxy",5000,-1,1);
    AddAxis("pt");
    AddAxis("nTracks","mult6kcoarse");
    AddAxis("cent");
    AddAxis("MCinfo",4,-1.5,2.5); // 0=prim, 1=decay 2=material -1=data
    fHistDCA = CreateHist("fHistDCA");
    fOutputList->Add(fHistDCA);

    //dcar:pt:mult:cent:mcinfo
    AddAxis("DCAxy",5000,-20,20);
    AddAxis("TPCpt","pt");
    AddAxis("nTracks","mult6kcoarse");
    AddAxis("cent");
    AddAxis("MCinfo",4,-1.5,2.5); // 0=prim, 1=decay 2=material -1=data
    fHistDCATPC = CreateHist("fHistDCATPC");
    fOutputList->Add(fHistDCATPC);

     //dcar:pt:mult:mcinfo

}

//_____________________________________________________________________________

Bool_t AliAnalysisTaskDCArStudy::IsEventSelected()
{
    return fIsAcceptedAliEventCuts;
}

//_____________________________________________________________________________

void AliAnalysisTaskDCArStudy::AnaEvent()
{
    LoopOverAllTracks();
}

//_____________________________________________________________________________

void AliAnalysisTaskDCArStudy::AnaTrackMC(Int_t flag)
{
    if (fAcceptTrack[0]) { FillHist(fHistDCATPC, fDCArTPC, fPtInnerTPC, fNTracksAcc, fMultPercentileV0M, fMCPrimSec); }
    if (fAcceptTrack[1]) { FillHist(fHistDCA, fDCAr, fPt, fNTracksAcc, fMultPercentileV0M, fMCPrimSec); }
}

//_____________________________________________________________________________

void AliAnalysisTaskDCArStudy::AnaTrackDATA(Int_t flag)
{
    if (fAcceptTrack[0]) { FillHist(fHistDCATPC, fDCArTPC, fPtInnerTPC, fNTracksAcc, fMultPercentileV0M, -1); }
    if (fAcceptTrack[1]) { FillHist(fHistDCA, fDCAr, fPt, fNTracksAcc, fMultPercentileV0M, -1); }
}

//_____________________________________________________________________________

AliAnalysisTaskDCArStudy* AliAnalysisTaskDCArStudy::AddTaskDCArStudy(const char* name, const char* outfile)
{
    AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
    if (!mgr) {
        ::Error("AddTaskDCArStudy", "No analysis manager to connect to.");
        return 0;
    }

    // Check the analysis type using the event handlers connected to the analysis manager.
    //==============================================================================
    if (!mgr->GetInputEventHandler()) {
        ::Error("AddTaskDCArStudy", "This task requires an input event handler");
        return NULL;
    }

    // Setup output file
    //===========================================================================
    TString fileName = AliAnalysisManager::GetCommonFileName();
    fileName += ":";
    fileName += name;  // create a subfolder in the file
    if (outfile) { // if a finename is given, use that one
        fileName = TString(outfile);
    }

    // create the task
    //===========================================================================
    AliAnalysisTaskDCArStudy *task = new AliAnalysisTaskDCArStudy(name);
    if (!task) { return 0; }

    // configure the task
    //===========================================================================
    task->SelectCollisionCandidates(AliVEvent::kAnyINT);
    task->SetESDtrackCutsM(AlidNdPtTools::CreateESDtrackCuts("defaultEta08"));
    task->SetESDtrackCuts(0,AlidNdPtTools::CreateESDtrackCuts("TPCgeoNoDCArEta08"));
    task->SetESDtrackCuts(1,AlidNdPtTools::CreateESDtrackCuts("TPCITSforDCArStudyEta08"));
    task->SetNeedEventMult(kTRUE);
    task->SetNeedEventVertex(kTRUE);
    task->SetNeedTrackTPC(kTRUE);

    // attach the task to the manager and configure in and ouput
    //===========================================================================
    mgr->AddTask(task);
    mgr->ConnectInput(task,0,mgr->GetCommonInputContainer());
    mgr->ConnectOutput(task,1,mgr->CreateContainer(name, TList::Class(), AliAnalysisManager::kOutputContainer, fileName.Data()));

  return task;
}
<commit_msg>Updated dca study for dNdPt framework: changed mult. binning<commit_after>#include <iostream>
#include "TChain.h"
#include "TH1F.h"
#include "TList.h"
#include "TGeoGlobalMagField.h"
#include "AliAnalysisTask.h"
#include "AliAnalysisManager.h"
#include "AliESDEvent.h"
#include "AliESDInputHandler.h"
#include "AliESDtrack.h"
#include "AliAODEvent.h"
#include "AliHeader.h"
#include "AliMCEvent.h"
#include "AliGenEventHeader.h"
#include "AliESDtrackCuts.h"
#include "AlidNdPtTools.h"
#include "AliAnalysisTaskMKBase.h"
#include "AliAnalysisTaskDCArStudy.h"

class AliAnalysisTaskDCArStudy;

using namespace std;

ClassImp(AliAnalysisTaskDCArStudy)

//_____________________________________________________________________________

AliAnalysisTaskDCArStudy::AliAnalysisTaskDCArStudy()
    : AliAnalysisTaskMKBase()
    , fHistDCA(0)
    , fHistDCATPC(0)
{
    // default contructor

}

//_____________________________________________________________________________

AliAnalysisTaskDCArStudy::AliAnalysisTaskDCArStudy(const char* name)
    : AliAnalysisTaskMKBase(name)
    , fHistDCA(0)
    , fHistDCATPC(0)
{
    // constructor

}

//_____________________________________________________________________________

AliAnalysisTaskDCArStudy::~AliAnalysisTaskDCArStudy()
{
    // destructor
}

//_____________________________________________________________________________

void AliAnalysisTaskDCArStudy::AddOutput()
{
    //dcar:pt:mult:mcinfo
    AddAxis("DCAxy",5000,-1,1);
    AddAxis("pt");
    AddAxis("nTracks",4000, -0.5, 3999.5);
    AddAxis("MCinfo",4,-1.5,2.5); // 0=prim, 1=decay 2=material -1=data
    fHistDCA = CreateHist("fHistDCA");
    fOutputList->Add(fHistDCA);

    //dcar:pt:mult:mcinfo
    AddAxis("DCAxy",5000,-20,20);
    AddAxis("TPCpt","pt");
    AddAxis("nTracks",4000, -0.5, 3999.5);
    AddAxis("MCinfo",4,-1.5,2.5); // 0=prim, 1=decay 2=material -1=data
    fHistDCATPC = CreateHist("fHistDCATPC");
    fOutputList->Add(fHistDCATPC);

     //dcar:pt:mult:mcinfo

}

//_____________________________________________________________________________

Bool_t AliAnalysisTaskDCArStudy::IsEventSelected()
{
    return fIsAcceptedAliEventCuts;
}

//_____________________________________________________________________________

void AliAnalysisTaskDCArStudy::AnaEvent()
{
    LoopOverAllTracks();
}

//_____________________________________________________________________________

void AliAnalysisTaskDCArStudy::AnaTrackMC(Int_t flag)
{
    if (fAcceptTrack[0]) { FillHist(fHistDCATPC, fDCArTPC, fPtInnerTPC, fNTracksAcc, fMCPrimSec); }
    if (fAcceptTrack[1]) { FillHist(fHistDCA, fDCAr, fPt, fNTracksAcc, fMCPrimSec); }
}

//_____________________________________________________________________________

void AliAnalysisTaskDCArStudy::AnaTrackDATA(Int_t flag)
{
    if (fAcceptTrack[0]) { FillHist(fHistDCATPC, fDCArTPC, fPtInnerTPC, fNTracksAcc, -1); }
    if (fAcceptTrack[1]) { FillHist(fHistDCA, fDCAr, fPt, fNTracksAcc, -1); }
}

//_____________________________________________________________________________

AliAnalysisTaskDCArStudy* AliAnalysisTaskDCArStudy::AddTaskDCArStudy(const char* name, const char* outfile)
{
    AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
    if (!mgr) {
        ::Error("AddTaskDCArStudy", "No analysis manager to connect to.");
        return 0;
    }

    // Check the analysis type using the event handlers connected to the analysis manager.
    //==============================================================================
    if (!mgr->GetInputEventHandler()) {
        ::Error("AddTaskDCArStudy", "This task requires an input event handler");
        return NULL;
    }

    // Setup output file
    //===========================================================================
    TString fileName = AliAnalysisManager::GetCommonFileName();
    fileName += ":";
    fileName += name;  // create a subfolder in the file
    if (outfile) { // if a finename is given, use that one
        fileName = TString(outfile);
    }

    // create the task
    //===========================================================================
    AliAnalysisTaskDCArStudy *task = new AliAnalysisTaskDCArStudy(name);
    if (!task) { return 0; }

    // configure the task
    //===========================================================================
    task->SelectCollisionCandidates(AliVEvent::kAnyINT);
    task->SetESDtrackCutsM(AlidNdPtTools::CreateESDtrackCuts("defaultEta08"));
    task->SetESDtrackCuts(0,AlidNdPtTools::CreateESDtrackCuts("TPCgeoNoDCArEta08"));
    task->SetESDtrackCuts(1,AlidNdPtTools::CreateESDtrackCuts("TPCITSforDCArStudyEta08"));
    task->SetNeedEventMult(kTRUE);
    task->SetNeedEventVertex(kTRUE);
    task->SetNeedTrackTPC(kTRUE);

    // attach the task to the manager and configure in and ouput
    //===========================================================================
    mgr->AddTask(task);
    mgr->ConnectInput(task,0,mgr->GetCommonInputContainer());
    mgr->ConnectOutput(task,1,mgr->CreateContainer(name, TList::Class(), AliAnalysisManager::kOutputContainer, fileName.Data()));

  return task;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <fstream>

#define CAMERA 0
#define VWIDTH 640
#define VHEIGHT 480
#define HORIZ 1
#define RADIUS 10

using namespace cv;
using namespace std;

Scalar linecolor = Scalar(0,255,0);
Scalar rectcolor = Scalar(0,200,0);

//Function to find the centroid of a binary image
void getCentroid(Mat &thresholded_image, Point &Centroid, int &Area)
{
    ///The object that holds all the centroids.
    ///Pass in the image. The boolean true tells the function that
    ///the image is binary
    Moments m = moments(thresholded_image, true);
    ///Moment along x axis
    double M10 = m.m10;
    ///Moment along y-axis;
    double M01 = m.m01;
    ///Area
    double M00 = m.m00;
    Centroid.x  = int(M10/M00);
    Centroid.y  = int(M01/M00);
    Area        = int(M00);
}

int main(int argc, char** argv)
{
    Mat frame, head, tail;
    char keypress;

    int rlower_h = 0;   int glower_h = 0;   int blower_h = 0;
    int rupper_h = 255; int gupper_h = 255; int bupper_h = 255;

    int rlower_t = 0;   int glower_t = 0;   int blower_t = 0;
    int rupper_t = 255; int gupper_t = 255; int bupper_t = 255;

    int max_thresh = 255;

    Scalar head_lower, head_upper,
	tail_lower, tail_upper;
    Point head_centroid, tail_centroid, direction;
    Point head_prev(0,0); Point motion(0,0);
    int head_area, tail_area;

    namedWindow("head");
    namedWindow("tail");
    createTrackbar("R Upper", "head", &rupper_h, max_thresh, NULL);
    createTrackbar("R Lower", "head", &rlower_h, max_thresh, NULL);
    createTrackbar("G Upper", "head", &gupper_h, max_thresh, NULL);
    createTrackbar("G Lower", "head", &glower_h, max_thresh, NULL);
    createTrackbar("B Upper", "head", &bupper_h, max_thresh, NULL);
    createTrackbar("B Lower", "head", &blower_h, max_thresh, NULL);

    createTrackbar("R Upper", "tail", &rupper_t, max_thresh, NULL);
    createTrackbar("R Lower", "tail", &rlower_t, max_thresh, NULL);
    createTrackbar("G Upper", "tail", &gupper_t, max_thresh, NULL);
    createTrackbar("G Lower", "tail", &glower_t, max_thresh, NULL);
    createTrackbar("B Upper", "tail", &bupper_t, max_thresh, NULL);
    createTrackbar("B Lower", "tail", &blower_t, max_thresh, NULL);

    VideoCapture camera;

    camera.open(CAMERA);
    //Setting the camera resolution
    camera.set(CV_CAP_PROP_FRAME_WIDTH, VWIDTH);
    camera.set(CV_CAP_PROP_FRAME_HEIGHT, VHEIGHT);

    while(1)
    {
	//Get frame from camera
	camera >> frame;
	flip(frame, frame, HORIZ);
        //Threshold out the green marker(tail)
	//Threshold out the red marker(head)
	inRange(frame, Scalar(rlower_h, glower_h, blower_h),
		Scalar(rupper_h, gupper_h, bupper_h), head);
	inRange(frame, Scalar(rlower_t, glower_t, blower_t),
		Scalar(rupper_t, gupper_t, bupper_t), tail);
	medianBlur(head, head, 7);

	//Get coordinates of the tail marker
	getCentroid(tail, tail_centroid, tail_area);
	//Get coordinates of the head marker
	getCentroid(head, head_centroid, head_area);
	//Subtract the coordinates to get a direction vector
	direction = head_centroid - tail_centroid;

	//Draw the appropriate stuff on the image
	motion = head_centroid - head_prev;
	line(frame, tail_centroid, head_centroid, linecolor);
	circle(frame, head_centroid, RADIUS, linecolor);

	//cout<<"Direction: "<<direction<<endl;
	//Use the direction vector to decide which way the arm must move
	//Display the motion as output on the terminal
	imshow("head", head);
	imshow("tail", tail);
	imshow("frame", frame);

	head_prev = head_centroid;

	keypress = waitKey(10);
	if(keypress == 27) break;
	if(keypress == 's')
	{
	    //save thresholds

	    //Show directions
	    cout<<"Direction is: "<<direction<<endl;
	    break;
	}

	//Use the sliders to adjust the thresholds
	//Save the thresholds in variables
	//break out of loop
    }

    return 0;

}
<commit_msg>Sending commands to right is working.<commit_after>#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <fstream>
#include "include/netTransmit.h"
#include <math.h>

#define CAMERA 0
#define VWIDTH 640
#define VHEIGHT 480
#define HORIZ 1
#define RADIUS 10

using namespace cv;
using namespace std;

Scalar linecolor = Scalar(0,255,0);
Scalar rectcolor = Scalar(0,200,0);

//Function to convert from number to string
template <typename T>
string NumberToString (T Number)
{
    ostringstream ss;
    ss << Number;
    return ss.str();
}

//Function to find the centroid of a binary image
void getCentroid(Mat &thresholded_image, Point &Centroid, int &Area)
{
    ///The object that holds all the centroids.
    ///Pass in the image. The boolean true tells the function that
    ///the image is binary
    Moments m = moments(thresholded_image, true);
    ///Moment along x axis
    double M10 = m.m10;
    ///Moment along y-axis;
    double M01 = m.m01;
    ///Area
    double M00 = m.m00;
    Centroid.x  = int(M10/M00);
    Centroid.y  = int(M01/M00);
    Area        = int(M00);
}

//Simplified puttext
void ptext(Mat& img, const string& text, Point org)
{
    static double FSIZE = 0.5;
    static int THICKNESS = 1;
    putText(img, text, org, FONT_HERSHEY_SIMPLEX, FSIZE,
	    Scalar(0,255,0), THICKNESS, CV_AA);
}

int main(int argc, char** argv)
{
    Mat frame, head, tail;
    char keypress;

    int rlower_h = 0;   int glower_h = 0;   int blower_h = 0;
    int rupper_h = 255; int gupper_h = 255; int bupper_h = 255;

    //int rlower_t = 0;   int glower_t = 0;   int blower_t = 0;
    //int rupper_t = 255; int gupper_t = 255; int bupper_t = 255;

    int max_thresh = 255;

    Scalar head_lower, head_upper;
    //tail_lower, tail_upper;
    Point head_centroid, direction;
    double angle;
    Point tail_centroid = Point(VWIDTH/2, VHEIGHT/2);
    Point head_prev(0,0); Point motion(0,0);
    int head_area, tail_area;
    char *msg   = "Hello World!";
    char *up    = "U";
    char *down  = "D";
    char *left  = "L";
    char *right = "R";
    char *stop  = "S";
    string disp;

    //class to transmit the arm motion.
    netTransmit dataFire("127.0.0.1", 31415);

    namedWindow("head");
    //namedWindow("tail");
    createTrackbar("R Upper", "head", &rupper_h, max_thresh, NULL);
    createTrackbar("R Lower", "head", &rlower_h, max_thresh, NULL);
    createTrackbar("G Upper", "head", &gupper_h, max_thresh, NULL);
    createTrackbar("G Lower", "head", &glower_h, max_thresh, NULL);
    createTrackbar("B Upper", "head", &bupper_h, max_thresh, NULL);
    createTrackbar("B Lower", "head", &blower_h, max_thresh, NULL);

    //createTrackbar("R Upper", "tail", &rupper_t, max_thresh, NULL);
    //createTrackbar("R Lower", "tail", &rlower_t, max_thresh, NULL);
    //createTrackbar("G Upper", "tail", &gupper_t, max_thresh, NULL);
    //createTrackbar("G Lower", "tail", &glower_t, max_thresh, NULL);
    //createTrackbar("B Upper", "tail", &bupper_t, max_thresh, NULL);
    //createTrackbar("B Lower", "tail", &blower_t, max_thresh, NULL);

    VideoCapture camera;

    camera.open(CAMERA);
    //Setting the camera resolution
    camera.set(CV_CAP_PROP_FRAME_WIDTH, VWIDTH);
    camera.set(CV_CAP_PROP_FRAME_HEIGHT, VHEIGHT);

    while(1)
    {
	//Get frame from camera
	camera >> frame;
	flip(frame, frame, HORIZ);
        //Threshold out the green marker(tail)
	//Threshold out the red marker(head)
	inRange(frame, Scalar(rlower_h, glower_h, blower_h),
		Scalar(rupper_h, gupper_h, bupper_h), head);
	//inRange(frame, Scalar(rlower_t, glower_t, blower_t),
	//	Scalar(rupper_t, gupper_t, bupper_t), tail);
	medianBlur(head, head, 7);

	//Get coordinates of the tail marker
	//getCentroid(tail, tail_centroid, tail_area);
	//Get coordinates of the head marker
	getCentroid(head, head_centroid, head_area);
	//Subtract the coordinates to get a direction vector
	direction = head_centroid - tail_centroid;
	angle = (180.0/3.14159)*atan2(-1*direction.y, direction.x);
	//Draw the appropriate stuff on the image
	motion = head_centroid - head_prev;
	line(frame, tail_centroid, head_centroid, linecolor);
	circle(frame, head_centroid, RADIUS, linecolor);

	ptext(frame, NumberToString(angle), head_centroid+Point(20, 0));
	if((angle>0)&&(angle<45)) ptext(frame, string(right), Point(VWIDTH/2, 40));

	//cout<<"Direction: "<<direction<<endl;
	//Use the direction vector to decide which way the arm must move
	//Display the motion as output on the terminal
	imshow("head", head);
	imshow("frame", frame);

	head_prev = head_centroid;
	//dataFire.transmit(msg);

	keypress = waitKey(10);
	if(keypress == 27) break;
	if(keypress == 's')
	{
	    //save thresholds

	    //Show directions
	    cout<<"Direction is: "<<direction<<endl;
	    break;
	}

	//Use the sliders to adjust the thresholds
	//Save the thresholds in variables
	//break out of loop
    }

    return 0;

}
<|endoftext|>