<commit_before>////////////////////////////////////////////////////////////////////////////////
//
// This is a test program for Safe-Bit-Fields in the Loki Library.
// Copyright (c) 2009 by Fedor Pikus & Rich Sposato
// The copyright on this file is protected under the terms of the MIT license.
//
// Permission to use, copy, modify, distribute and sell this software for any
//     purpose is hereby granted without fee, provided that the above copyright
//     notice appear in all copies and that both that copyright notice and this
//     permission notice appear in supporting documentation.
// The author makes no claims about the suitability of this software for any
//     purpose. It is provided "as is" without express or implied warranty.
////////////////////////////////////////////////////////////////////////////////

// $Id$

#include <loki/SafeBits.h>

#include <iostream>

using namespace std;
using namespace Loki;

LOKI_BIT_FIELD( unsigned int ) Cat_state;
LOKI_BIT_CONST( Cat_state, CAT_NO_STATE, 0 );                        // 0x0000 - no bit is set
LOKI_BIT_CONST( Cat_state, CAT_SLEEPING, 1 );                        // 0x0001 - 1st bit is set
LOKI_BIT_CONST( Cat_state, CAT_PURRING,  2 );                        // 0x0002 - 2nd bit is set
LOKI_BIT_CONST( Cat_state, CAT_PLAYING,  3 );                        // 0x0004 - 3rd bit is set
LOKI_BIT_FIELD( unsigned int ) Dog_state;
LOKI_BIT_CONST( Dog_state, DOG_BARKING,  1 );
LOKI_BIT_CONST( Dog_state, DOG_CHEWING,  2 );
LOKI_BIT_CONST( Dog_state, DOG_DROOLING, 3 );

int main( void )
{
    cout << "Running tests on Loki safe bit fields." << endl;

    Cat_state cat_state = CAT_SLEEPING;
    assert( cat_state );
    Dog_state dog_state = DOG_DROOLING;
    assert( dog_state );
    bool happy = cat_state & ( CAT_SLEEPING | CAT_PURRING );    // OK
    assert( happy );

    assert( CAT_SLEEPING <  CAT_PURRING  );
    assert( CAT_SLEEPING <= CAT_SLEEPING );
    assert( CAT_SLEEPING <= CAT_PURRING  );
    assert( CAT_SLEEPING != CAT_PURRING  );
    assert( CAT_SLEEPING == CAT_SLEEPING );
    assert( CAT_PURRING  >= CAT_SLEEPING );
    assert( CAT_PURRING  >= CAT_PURRING  );

#ifdef ERROR1
    assert( DOG_DROOLING != CAT_SLEEPING );                   // Can't compare different types.
#endif
#ifdef ERROR2
    if ( cat_state & DOG_BARKING ) {}                         // Wrong bit type
#endif
#ifdef ERROR3
    if ( cat_state & CAT_SLEEPING == 0 ) {}                   // Operator precedence
#endif
#ifdef ERROR4
    if ( dog_state && DOG_BARKING ) {}                        // Logical &&
#endif
    if ( dog_state & DOG_BARKING ) {}                         // OK
#ifdef ERROR5
    Cat_state state0 = 0;                                     // Conversion from non-safe bits
#endif

    Cat_state state = CAT_NO_STATE;                           // OK
    assert( !state );
    state = ~state;
    assert( state );
    assert( state != CAT_NO_STATE );
    assert( state & CAT_SLEEPING );
    assert( state & CAT_PURRING );
    assert( state & CAT_PLAYING );
    state = CAT_SLEEPING;
    assert( state == cat_state );
    assert( state.size() == 8 * sizeof( unsigned int ) );
    assert( sizeof( Cat_state ) == sizeof( unsigned int ) );

    dog_state = DOG_BARKING;
#ifdef ERROR6
    if ( dog_state == cat_state ) {}                          // Don't allow comparison of different types.
#endif

/// @note All These assertions are inside #ifdef sections because they
/// compare either Safe_bit_field or Safe_bit_const to literal integers.
/// If you compile any of these assertions they should generate errors.
/// These #ifdef sections exhaustively demonstrate that all possible
/// operations and comparisons with literal values are forbidden.

#ifdef ERROR7
    assert( dog_state == 1 );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR8
    assert( dog_state != 2 );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR9
    assert( dog_state < 2 );                                  // Don't allow comparisons to integers.
#endif
#ifdef ERROR10
    assert( dog_state > 0 );                                  // Don't allow comparisons to integers.
#endif
#ifdef ERROR11
    assert( dog_state <= 1 );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR12
    assert( dog_state >= 1 );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR13
    assert( DOG_BARKING == 1 );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR14
    assert( DOG_BARKING != 2 );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR15
    assert( DOG_BARKING < 2 );                                // Don't allow comparisons to integers.
#endif
#ifdef ERROR16
    assert( DOG_BARKING > 0 );                                // Don't allow comparisons to integers.
#endif
#ifdef ERROR17
    assert( DOG_BARKING <= 1 );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR18
    assert( DOG_BARKING >= 1 );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR19
    assert( ( dog_state | 1 ) != 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR20
    assert( ( dog_state & 2 ) == 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR21
    assert( ( dog_state ^ 1 ) == 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR22
    assert( ( dog_state |= 2 ) == 3 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR23
    assert( ( dog_state &= 3 ) == 2 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR24
    assert( ( dog_state ^= 1 ) == 0 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR25
    assert( ( DOG_BARKING | 1 ) != 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR26
    assert( ( DOG_BARKING & 2 ) == 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR27
    assert( ( DOG_BARKING ^ 1 ) == 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR28
    assert( ( DOG_BARKING |= 2 ) == 3 );                      // Don't allow operations with integers.
#endif
#ifdef ERROR29
    assert( ( DOG_BARKING &= 3 ) == 2 );                      // Don't allow operations with integers.
#endif
#ifdef ERROR30
    assert( ( DOG_BARKING ^= 1 ) == 0 );                      // Don't allow operations with integers.
#endif

/// @note All These assertions are inside #ifdef sections because they
/// compare either Safe_bit_field or Safe_bit_const to an int variable.
/// If you compile any of these assertions they should generate errors.
/// These #ifdef sections exhaustively demonstrate that all possible
/// operations and comparisons with integers are forbidden.

    int value = 1;
    (void)value;
#ifdef ERROR31
    assert( dog_state == value );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR32
    value = 2;
    assert( dog_state != value );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR33
    value = 2;
    assert( dog_state < value );                                  // Don't allow comparisons to integers.
#endif
#ifdef ERROR34
    value = 0;
    assert( dog_state > value );                                  // Don't allow comparisons to integers.
#endif
#ifdef ERROR35
    value = 1;
    assert( dog_state <= value );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR36
    value = 1;
    assert( dog_state >= value );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR37
    value = 1;
    assert( DOG_BARKING == value );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR38
    value = 2;
    assert( DOG_BARKING != value );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR39
    value = 2;
    assert( DOG_BARKING < value );                                // Don't allow comparisons to integers.
#endif
#ifdef ERROR40
    value = 0;
    assert( DOG_BARKING > value );                                // Don't allow comparisons to integers.
#endif
#ifdef ERROR41
    value = 1;
    assert( DOG_BARKING <= value );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR42
    value = 1;
    assert( DOG_BARKING >= value );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR43
    value = 1;
    assert( ( dog_state | value ) != 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR44
    value = 2;
    assert( ( dog_state & value ) == 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR45
    value = 1;
    assert( ( dog_state ^ value ) == 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR46
    value = 2;
    assert( ( dog_state |= value ) == 3 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR47
    value = 3;
    assert( ( dog_state &= value ) == 2 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR48
    value = 1;
    assert( ( dog_state ^= value ) == 0 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR49
    value = 1;
    assert( ( DOG_BARKING | value ) != 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR50
    value = 2;
    assert( ( DOG_BARKING & value ) == 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR51
    value = 1;
    assert( ( DOG_BARKING ^ value ) == 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR52
    value = 2;
    assert( ( DOG_BARKING |= value ) == 3 );                      // Don't allow operations with integers.
#endif
#ifdef ERROR53
    value = 3;
    assert( ( DOG_BARKING &= value ) == 2 );                      // Don't allow operations with integers.
#endif
#ifdef ERROR54
    value = 1;
    assert( ( DOG_BARKING ^= value ) == 0 );                      // Don't allow operations with integers.
#endif

    dog_state |= DOG_CHEWING;
    assert( dog_state & ( DOG_CHEWING | DOG_BARKING ) );
    dog_state &= DOG_CHEWING;
    assert( dog_state == DOG_CHEWING );
    dog_state = ~dog_state;
    assert( dog_state != DOG_CHEWING );
    dog_state = ~dog_state;
    assert( dog_state == DOG_CHEWING );

    cout << "If nothing asserted, then all tests passed!" << endl;
    return 0;
}
<commit_msg>Added more code for testing.<commit_after>////////////////////////////////////////////////////////////////////////////////
//
// This is a test program for Safe-Bit-Fields in the Loki Library.
// Copyright (c) 2009 by Fedor Pikus & Rich Sposato
// The copyright on this file is protected under the terms of the MIT license.
//
// Permission to use, copy, modify, distribute and sell this software for any
//     purpose is hereby granted without fee, provided that the above copyright
//     notice appear in all copies and that both that copyright notice and this
//     permission notice appear in supporting documentation.
// The author makes no claims about the suitability of this software for any
//     purpose. It is provided "as is" without express or implied warranty.
////////////////////////////////////////////////////////////////////////////////

// $Id$

#include <loki/SafeBits.h>

#include <iostream>

using namespace std;
using namespace Loki;

LOKI_BIT_FIELD( unsigned int ) Cat_state;
LOKI_BIT_CONST( Cat_state, CAT_NO_STATE, 0 );                        // 0x0000 - no bit is set
LOKI_BIT_CONST( Cat_state, CAT_SLEEPING, 1 );                        // 0x0001 - 1st bit is set
LOKI_BIT_CONST( Cat_state, CAT_PURRING,  2 );                        // 0x0002 - 2nd bit is set
LOKI_BIT_CONST( Cat_state, CAT_PLAYING,  3 );                        // 0x0004 - 3rd bit is set
LOKI_BIT_FIELD( unsigned int ) Dog_state;
LOKI_BIT_CONST( Dog_state, DOG_BARKING,  1 );
LOKI_BIT_CONST( Dog_state, DOG_CHEWING,  2 );
LOKI_BIT_CONST( Dog_state, DOG_DROOLING, 3 );
LOKI_BIT_CONST( Dog_state, DOG_SLEEPING, 4 );
LOKI_BIT_CONST( Dog_state, DOG_GROWLING, 5 );
LOKI_BIT_CONST( Dog_state, DOG_TIRED,    6 );
LOKI_BIT_CONST( Dog_state, DOG_DEAD,     7 );
LOKI_BIT_CONST( Dog_state, DOG_GROWING,  8 );
LOKI_BIT_CONST( Dog_state, DOG_THINKING, 9 );
LOKI_BIT_CONST( Dog_state, DOG_YELPING,  10 );
LOKI_BIT_CONST( Dog_state, DOG_QUIET,    11 );
LOKI_BIT_CONST( Dog_state, DOG_FETCHING, 12 );
LOKI_BIT_CONST( Dog_state, DOG_HOWLING,  13 );
LOKI_BIT_CONST( Dog_state, DOG_SICK,     14 );
LOKI_BIT_CONST( Dog_state, DOG_JUMPING,  15 );
LOKI_BIT_CONST( Dog_state, DOG_SWIMMING, 16 );
LOKI_BIT_CONST( Dog_state, DOG_BATHING,  17 );
LOKI_BIT_CONST( Dog_state, DOG_EATING,   18 );
LOKI_BIT_CONST( Dog_state, DOG_DRINKING, 19 );
LOKI_BIT_CONST( Dog_state, DOG_PLAYING,  20 );
LOKI_BIT_CONST( Dog_state, DOG_RUNNING,  21 );
LOKI_BIT_CONST( Dog_state, DOG_BITING,   22 );
LOKI_BIT_CONST( Dog_state, DOG_BEGGING,  23 );
LOKI_BIT_CONST( Dog_state, DOG_WHINING,  24 );
LOKI_BIT_CONST( Dog_state, DOG_WALKING,  25 );
LOKI_BIT_CONST( Dog_state, DOG_SINGING,  26 );
LOKI_BIT_CONST( Dog_state, DOG_FIGHTING, 27 );
LOKI_BIT_CONST( Dog_state, DOG_MATING,   28 );
LOKI_BIT_CONST( Dog_state, DOG_FEEDING,  29 );
LOKI_BIT_CONST( Dog_state, DOG_BIRTHING, 30 );
LOKI_BIT_CONST( Dog_state, DOG_SHEDDING, 31 );
LOKI_BIT_CONST( Dog_state, DOG_TALKING,  32 );

#ifdef ERROR0
    LOKI_BIT_CONST( Dog_state, DOG_TALKING,  20 ); // Can't have two values with same name.
    LOKI_BIT_CONST( Dog_state, DOG_BLOGGING, 33 ); // Can't set bit 33 in a 32 bit-sized object.
#endif

int main( void )
{
    cout << "Running tests on Loki safe bit fields." << endl;

    Cat_state cat_state = CAT_SLEEPING;
    assert( cat_state );
    Dog_state dog_state = DOG_DROOLING;
    assert( dog_state );
    bool happy = cat_state & ( CAT_SLEEPING | CAT_PURRING );    // OK
    assert( happy );

    assert( CAT_SLEEPING <  CAT_PURRING  );
    assert( CAT_SLEEPING <= CAT_SLEEPING );
    assert( CAT_SLEEPING <= CAT_PURRING  );
    assert( CAT_SLEEPING != CAT_PURRING  );
    assert( CAT_SLEEPING == CAT_SLEEPING );
    assert( CAT_PURRING  >= CAT_SLEEPING );
    assert( CAT_PURRING  >= CAT_PURRING  );

#ifdef ERROR1
    assert( DOG_DROOLING != CAT_SLEEPING );                   // Can't compare different types.
#endif
#ifdef ERROR2
    if ( cat_state & DOG_BARKING ) {}                         // Wrong bit type
#endif
#ifdef ERROR3
    if ( cat_state & CAT_SLEEPING == 0 ) {}                   // Operator precedence
#endif
#ifdef ERROR4
    if ( dog_state && DOG_BARKING ) {}                        // Logical &&
#endif
    if ( dog_state & DOG_BARKING ) {}                         // OK
#ifdef ERROR5
    Cat_state state0 = 0;                                     // Conversion from non-safe bits
#endif

    Cat_state state = CAT_NO_STATE;                           // OK
    assert( !state );
    state = ~state;
    assert( state );
    assert( state != CAT_NO_STATE );
    assert( state & CAT_SLEEPING );
    assert( state & CAT_PURRING );
    assert( state & CAT_PLAYING );
    state = CAT_SLEEPING;
    assert( state == cat_state );
    assert( state.size() == ( 8 * sizeof(unsigned int) ) );
    assert( sizeof(Cat_state) == sizeof(unsigned int) );

    dog_state = DOG_BARKING;
#ifdef ERROR6
    if ( dog_state == cat_state ) {}                          // Don't allow comparison of different types.
#endif

/// @note All These assertions are inside #ifdef sections because they
/// compare either SafeBitField or SafeBitConst to literal integers.
/// If you compile any of these assertions they should generate errors.
/// These #ifdef sections exhaustively demonstrate that all possible
/// operations and comparisons with literal values are forbidden.

#ifdef ERROR7
    assert( dog_state == 1 );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR8
    assert( dog_state != 2 );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR9
    assert( dog_state < 2 );                                  // Don't allow comparisons to integers.
#endif
#ifdef ERROR10
    assert( dog_state > 0 );                                  // Don't allow comparisons to integers.
#endif
#ifdef ERROR11
    assert( dog_state <= 1 );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR12
    assert( dog_state >= 1 );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR13
    assert( DOG_BARKING == 1 );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR14
    assert( DOG_BARKING != 2 );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR15
    assert( DOG_BARKING < 2 );                                // Don't allow comparisons to integers.
#endif
#ifdef ERROR16
    assert( DOG_BARKING > 0 );                                // Don't allow comparisons to integers.
#endif
#ifdef ERROR17
    assert( DOG_BARKING <= 1 );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR18
    assert( DOG_BARKING >= 1 );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR19
    assert( ( dog_state | 1 ) != 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR20
    assert( ( dog_state & 2 ) == 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR21
    assert( ( dog_state ^ 1 ) == 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR22
    assert( ( dog_state |= 2 ) == 3 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR23
    assert( ( dog_state &= 3 ) == 2 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR24
    assert( ( dog_state ^= 1 ) == 0 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR25
    assert( ( DOG_BARKING | 1 ) != 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR26
    assert( ( DOG_BARKING & 2 ) == 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR27
    assert( ( DOG_BARKING ^ 1 ) == 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR28
    assert( ( DOG_BARKING |= 2 ) == 3 );                      // Don't allow operations with integers.
#endif
#ifdef ERROR29
    assert( ( DOG_BARKING &= 3 ) == 2 );                      // Don't allow operations with integers.
#endif
#ifdef ERROR30
    assert( ( DOG_BARKING ^= 1 ) == 0 );                      // Don't allow operations with integers.
#endif

/// @note All These assertions are inside #ifdef sections because they
/// compare either SafeBitField or SafeBitConst to an int variable.
/// If you compile any of these assertions they should generate errors.
/// These #ifdef sections exhaustively demonstrate that all possible
/// operations and comparisons with integers are forbidden.

    int value = 1;
    (void)value;
#ifdef ERROR31
    assert( dog_state == value );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR32
    value = 2;
    assert( dog_state != value );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR33
    value = 2;
    assert( dog_state < value );                                  // Don't allow comparisons to integers.
#endif
#ifdef ERROR34
    value = 0;
    assert( dog_state > value );                                  // Don't allow comparisons to integers.
#endif
#ifdef ERROR35
    value = 1;
    assert( dog_state <= value );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR36
    value = 1;
    assert( dog_state >= value );                                 // Don't allow comparisons to integers.
#endif
#ifdef ERROR37
    value = 1;
    assert( DOG_BARKING == value );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR38
    value = 2;
    assert( DOG_BARKING != value );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR39
    value = 2;
    assert( DOG_BARKING < value );                                // Don't allow comparisons to integers.
#endif
#ifdef ERROR40
    value = 0;
    assert( DOG_BARKING > value );                                // Don't allow comparisons to integers.
#endif
#ifdef ERROR41
    value = 1;
    assert( DOG_BARKING <= value );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR42
    value = 1;
    assert( DOG_BARKING >= value );                               // Don't allow comparisons to integers.
#endif
#ifdef ERROR43
    value = 1;
    assert( ( dog_state | value ) != 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR44
    value = 2;
    assert( ( dog_state & value ) == 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR45
    value = 1;
    assert( ( dog_state ^ value ) == 0 );                         // Don't allow operations with integers.
#endif
#ifdef ERROR46
    value = 2;
    assert( ( dog_state |= value ) == 3 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR47
    value = 3;
    assert( ( dog_state &= value ) == 2 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR48
    value = 1;
    assert( ( dog_state ^= value ) == 0 );                        // Don't allow operations with integers.
#endif
#ifdef ERROR49
    value = 1;
    assert( ( DOG_BARKING | value ) != 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR50
    value = 2;
    assert( ( DOG_BARKING & value ) == 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR51
    value = 1;
    assert( ( DOG_BARKING ^ value ) == 0 );                       // Don't allow operations with integers.
#endif
#ifdef ERROR52
    value = 2;
    assert( ( DOG_BARKING |= value ) == 3 );                      // Don't allow operations with integers.
#endif
#ifdef ERROR53
    value = 3;
    assert( ( DOG_BARKING &= value ) == 2 );                      // Don't allow operations with integers.
#endif
#ifdef ERROR54
    value = 1;
    assert( ( DOG_BARKING ^= value ) == 0 );                      // Don't allow operations with integers.
#endif

    dog_state |= DOG_CHEWING;
    assert( dog_state & ( DOG_CHEWING | DOG_BARKING ) );
    dog_state &= DOG_CHEWING;
    assert( dog_state == DOG_CHEWING );
    dog_state = ~dog_state;
    assert( dog_state != DOG_CHEWING );
    dog_state = ~dog_state;
    assert( dog_state == DOG_CHEWING );

    cout << "If nothing asserted, then all tests passed!" << endl;
    return 0;
}
<|endoftext|>


<commit_before>#include "fountain.h"
#include "TestApplication.h"

using namespace fei;

Texture tex;
Camera cam;
TestApplication testApp;
Clock mainClock;
ShaderProgram shader;
Image image, image2;

void test()
{
	//auto eg = testApp.getEngine();
	//if (eg->window->getKey(GLFW_KEY_W)) speed += fei::Vec2(0.0f, 2.0f);
	//if (eg->window->getKey(GLFW_KEY_S)) speed += fei::Vec2(0.0f, -2.0f);
	//if (eg->window->getKey(GLFW_KEY_A)) speed += fei::Vec2(-2.0f, 0.0f);
	//if (eg->window->getKey(GLFW_KEY_D)) speed += fei::Vec2(2.0f, 0.0f);
	Vec2 speed(0);
	auto *joystick = fei::Control::getInstance()->getJoystick();
	if (joystick) {
		speed = joystick->getAxes() * 500.0f * (float)mainClock.getDeltaTime();
		image.setAngle(speed.getAngle());
		speed = joystick->getDirection() * 50.0f * (float)mainClock.getDeltaTime();
		tex.rotate(-speed.x);
	}
	//cam.setPosition(pos);
	cam.update();
	tex.draw();
	image.draw();
	image2.draw();
	//char *buffer = fei::readFileBuffer("1.txt");
	//if (buffer) {
		//std::printf("%s", buffer);
		//delete [] buffer;
	//}
	//std::printf("Time: %f    Frame: %lld\n", fei::Time::getInstance()->getTime(), fei::Time::getInstance()->getFrame());
	mainClock.tick();
}

void TestApplication::engineSetting(fei::Engine *eg)
{
	if (!eg) return;
	eg->window->setSize(800, 600);
	eg->window->setTitle("fountain-tests");
	eg->window->setResizable(false);

	eg->setFrameFunc(test);

	Render::getInstance()->setViewport(fei::Rect(0, 0, 800, 600));

	cam.setCameraSize(fei::Vec2(4000, 3000));
	tex.loadFile("test.png");
	image = tex.getImage(fei::Vec2(100.0f), fei::Vec2(200.0f));
	image2 = image.getImage(fei::Vec2(0.0f), fei::Vec2(100.0f));
	image.setPosition(fei::Vec2(0.0f, 256.0f));
	shader.loadFile("vs.vert", "fs.frag");
	tex.setScale(2.0f);
	tex.setAnchor(Vec2(0.0f, -256.0f));
	//tex.setShader(&shader);

	//Math::getInstance()->setRandomSeed(9312);
	//Render::getInstance()->setClearColor(FEI_Blue);
	//Scene::getInstance()->gotoScene(new TestScene());
}

int main()
{
	testApp.run();
	return 0;
}
<commit_msg>edit fountain-test<commit_after>#include "fountain.h"
#include "TestApplication.h"

using namespace fei;

Texture tex;
Camera cam;
TestApplication testApp;
Clock mainClock;
ShaderProgram shader;
Image image, image2;

void test()
{
	auto *joystick = fei::Control::getInstance()->getJoystick();
	if (joystick) {
		Vec2 speed = joystick->getAxes() * 500.0f * (float)mainClock.getDeltaTime();
		image.setAngle(speed.getAngle());
		speed = joystick->getDirection() * 50.0f * (float)mainClock.getDeltaTime();
		tex.rotate(-speed.x);
	}
	cam.update();
	tex.draw();
	image.draw();
	image2.draw();
	mainClock.tick();
}

void TestApplication::engineSetting(fei::Engine *eg)
{
	if (!eg) return;
	eg->window->setSize(800, 600);
	eg->window->setTitle("fountain-tests");
	eg->window->setResizable(false);

	eg->setFrameFunc(test);

	Render::getInstance()->setViewport(fei::Rect(0, 0, 800, 600));

	cam.setCameraSize(fei::Vec2(800, 600));
	tex.loadFile("test.png");
	image = tex.getImage(fei::Vec2(100.0f), fei::Vec2(200.0f));
	image2 = image.getImage(fei::Vec2(0.0f), fei::Vec2(100.0f));
	image.setPosition(fei::Vec2(0.0f, 256.0f));
	shader.loadFile("vs.vert", "fs.frag");
	tex.setScale(0.4f);
	tex.setAnchor(Vec2(0.0f, -256.0f));
	tex.setShader(&shader);

	//Math::getInstance()->setRandomSeed(9312);
	//Render::getInstance()->setClearColor(FEI_Blue);
	//Scene::getInstance()->gotoScene(new TestScene());
}

int main()
{
	testApp.run();
	return 0;
}
<|endoftext|>


<commit_before>/*
 ============================================================================
 Name        : DmdSignal.cpp
 Author      : weizhenwei, <weizhenwei1988@gmail.com>
 Date           :2016.02.13
 Copyright   :
 * Copyright (c) 2016, weizhenwei
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the {organization} nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Description : signal processing implementation file.
 ============================================================================
 */

#include <assert.h>
#include <signal.h>

#include <string>

#include "DmdLog.h"

#include "DmdSignal.h"

namespace opendmd {

void DmdRegisterDefaultSignal() {
    signal(SIGPIPE, SIG_IGN);
    signal(SIGTERM, SIG_DFL);
    signal(SIGHUP, SIG_DFL);
}

void DmdRegisterSignalHandler(int sig, DmdSignalHandler pSigHandler) {
    signal(sig, pSigHandler);
}

/*
 *  No Name      Default Action    Description
 *  2  SIGINT    terminate process interrupt program
 *  3  SIGQUIT   create core image quit program
 *  4  SIGILL    create core image illegal instruction
 *  5  SIGTRAP   create core image trace trap
 *  6  SIGABRT   create core image abort program (formerly SIGIOT)
 *  7  SIGEMT    create core image emulate instruction executed
 *  8  SIGFPE    create core image floating-point exception
 *  9  SIGKILL   terminate process kill program
 *  10 SIGBUS    create core image bus error
 *  11 SIGSEGV   create core image segmentation violation
 *  12 SIGSYS    create core image non-existent system call invoked
 *  13 SIGPIPE   terminate process write on a pipe with no reader
 *  14 SIGALRM   terminate process real-time timer expired
 *  15 SIGTERM   terminate process software termination signal
 *  16 SIGURG    discard signal    urgent condition present on socket
 *  17 SIGSTOP   stop process      stop (cannot be caught or ignored)
 *  18 SIGTSTP   stop process      stop signal generated from keyboard
 *  19 SIGCONT   discard signal    continue after stop
 *  20 SIGCHLD   discard signal    child status has changed
 *  21 SIGTTIN   stop process      background read attempted from control terminal
 *  22 SIGTTOU   stop process      background write attempted to control terminal
 *  23 SIGIO     discard signal    I/O is possible on a descriptor (see fcntl(2))
 *  24 SIGXCPU   terminate process cpu time limit exceeded (see setrlimit(2))
 *  25 SIGXFSZ   terminate process file size limit exceeded (see setrlimit(2))
 *  26 SIGVTALRM terminate process virtual time alarm (see setitimer(2))
 *  27 SIGPROF   terminate process profiling timer alarm (see setitimer(2))
 *  28 SIGWINCH  discard signal    Window size change
 *  29 SIGINFO   discard signal    status request from keyboard
 *  30 SIGUSR1   terminate process User defined signal 1
 *  31 SIGUSR2   terminate process User defined signal 2
 */
string DmdSignalToString(int sig) {
    if (sig == SIGHUP) {
        return "SIGHUP";
    } else if (sig == SIGINT) {
        return "SIGINT";
    } else if (sig == SIGQUIT) {
        return "SIGQUIT";
    } else if (sig == SIGILL) {
        return "SIGILL";
    } else if (sig == SIGTRAP) {
        return "SIGTRAP";
    } else if (sig == SIGABRT) {
        return "SIGABRT";
    } else if (sig == SIGEMT) {
        return "SIGEMT";
    } else if (sig == SIGFPE) {
        return "SIGFPE";
    } else if (sig == SIGKILL) {
        return "SIGKILL";
    } else if (sig == SIGBUS) {
        return "SIGBUS";
    } else if (sig == SIGSEGV) {
        return "SIGSEGV";
    } else if (sig == SIGSYS) {
        return "SIGSYS";
    } else if (sig == SIGPIPE) {
        return "SIGPIPE";
    } else if (sig == SIGALRM) {
        return "SIGALRM";
    } else if (sig == SIGTERM) {
        return "SIGTERM";
    } else if (sig == SIGURG) {
        return "SIGURG";
    } else if (sig == SIGSTOP) {
        return "SIGSTOP";
    } else if (sig == SIGTSTP) {
        return "SIGTSTP";
    } else if (sig == SIGCONT) {
        return "SIGCONT";
    } else if (sig == SIGCHLD) {
        return "SIGCHLD";
    } else if (sig == SIGTTIN) {
        return "SIGTTIN";
    } else if (sig == SIGTTOU) {
        return "SIGTTOU";
    } else if (sig == SIGIO) {
        return "SIGIO";
    } else if (sig == SIGXCPU) {
        return "SIGXCPU";
    } else if (sig == SIGXFSZ) {
        return "SIGXFSZ";
    } else if (sig == SIGVTALRM) {
        return "SIGVTALRM";
    } else if (sig == SIGPROF) {
        return "SIGPROF";
    } else if (sig == SIGWINCH) {
        return "SIGWINCH";
    } else if (sig == SIGINFO) {
        return "SIGINFO";
    } else if (sig == SIGUSR1) {
        return "SIGUSR1";
    } else if (sig == SIGUSR2) {
        return "SIGUSR2";
    } else {
        return "Unknown signal";
    }
}

}  // namespace opendmd

<commit_msg>fix compile error on linux<commit_after>/*
 ============================================================================
 Name        : DmdSignal.cpp
 Author      : weizhenwei, <weizhenwei1988@gmail.com>
 Date           :2016.02.13
 Copyright   :
 * Copyright (c) 2016, weizhenwei
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the {organization} nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Description : signal processing implementation file.
 ============================================================================
 */

#include <assert.h>
#include <signal.h>

#include <string>

#include "DmdLog.h"

#include "DmdSignal.h"

namespace opendmd {

void DmdRegisterDefaultSignal() {
    signal(SIGPIPE, SIG_IGN);
    signal(SIGTERM, SIG_DFL);
    signal(SIGHUP, SIG_DFL);
}

void DmdRegisterSignalHandler(int sig, DmdSignalHandler pSigHandler) {
    signal(sig, pSigHandler);
}

/*
 *  No Name      Default Action    Description
 *  2  SIGINT    terminate process interrupt program
 *  3  SIGQUIT   create core image quit program
 *  4  SIGILL    create core image illegal instruction
 *  5  SIGTRAP   create core image trace trap
 *  6  SIGABRT   create core image abort program (formerly SIGIOT)
 *  7  SIGEMT    create core image emulate instruction executed
 *  8  SIGFPE    create core image floating-point exception
 *  9  SIGKILL   terminate process kill program
 *  10 SIGBUS    create core image bus error
 *  11 SIGSEGV   create core image segmentation violation
 *  12 SIGSYS    create core image non-existent system call invoked
 *  13 SIGPIPE   terminate process write on a pipe with no reader
 *  14 SIGALRM   terminate process real-time timer expired
 *  15 SIGTERM   terminate process software termination signal
 *  16 SIGURG    discard signal    urgent condition present on socket
 *  17 SIGSTOP   stop process      stop (cannot be caught or ignored)
 *  18 SIGTSTP   stop process      stop signal generated from keyboard
 *  19 SIGCONT   discard signal    continue after stop
 *  20 SIGCHLD   discard signal    child status has changed
 *  21 SIGTTIN   stop process      background read attempted from control terminal
 *  22 SIGTTOU   stop process      background write attempted to control terminal
 *  23 SIGIO     discard signal    I/O is possible on a descriptor (see fcntl(2))
 *  24 SIGXCPU   terminate process cpu time limit exceeded (see setrlimit(2))
 *  25 SIGXFSZ   terminate process file size limit exceeded (see setrlimit(2))
 *  26 SIGVTALRM terminate process virtual time alarm (see setitimer(2))
 *  27 SIGPROF   terminate process profiling timer alarm (see setitimer(2))
 *  28 SIGWINCH  discard signal    Window size change
 *  29 SIGINFO   discard signal    status request from keyboard
 *  30 SIGUSR1   terminate process User defined signal 1
 *  31 SIGUSR2   terminate process User defined signal 2
 */
string DmdSignalToString(int sig) {
    if (sig == SIGHUP) {
        return "SIGHUP";
    } else if (sig == SIGINT) {
        return "SIGINT";
    } else if (sig == SIGQUIT) {
        return "SIGQUIT";
    } else if (sig == SIGILL) {
        return "SIGILL";
    } else if (sig == SIGTRAP) {
        return "SIGTRAP";
    } else if (sig == SIGABRT) {
        return "SIGABRT";
    /*
    } else if (sig == SIGEMT) {  // didn't define on linux;
        return "SIGEMT";
    */
    } else if (sig == SIGFPE) {
        return "SIGFPE";
    } else if (sig == SIGKILL) {
        return "SIGKILL";
    } else if (sig == SIGBUS) {
        return "SIGBUS";
    } else if (sig == SIGSEGV) {
        return "SIGSEGV";
    } else if (sig == SIGSYS) {
        return "SIGSYS";
    } else if (sig == SIGPIPE) {
        return "SIGPIPE";
    } else if (sig == SIGALRM) {
        return "SIGALRM";
    } else if (sig == SIGTERM) {
        return "SIGTERM";
    } else if (sig == SIGURG) {
        return "SIGURG";
    } else if (sig == SIGSTOP) {
        return "SIGSTOP";
    } else if (sig == SIGTSTP) {
        return "SIGTSTP";
    } else if (sig == SIGCONT) {
        return "SIGCONT";
    } else if (sig == SIGCHLD) {
        return "SIGCHLD";
    } else if (sig == SIGTTIN) {
        return "SIGTTIN";
    } else if (sig == SIGTTOU) {
        return "SIGTTOU";
    } else if (sig == SIGIO) {
        return "SIGIO";
    } else if (sig == SIGXCPU) {
        return "SIGXCPU";
    } else if (sig == SIGXFSZ) {
        return "SIGXFSZ";
    } else if (sig == SIGVTALRM) {
        return "SIGVTALRM";
    } else if (sig == SIGPROF) {
        return "SIGPROF";
    } else if (sig == SIGWINCH) {
        return "SIGWINCH";
    /*
    } else if (sig == SIGINFO) {  // didn't define on linux;
        return "SIGINFO";
    */
    } else if (sig == SIGUSR1) {
        return "SIGUSR1";
    } else if (sig == SIGUSR2) {
        return "SIGUSR2";
    } else {
        return "Unknown signal";
    }
}

}  // namespace opendmd

<|endoftext|>


<commit_before>/*! \class   TrackFitPCAProducer
 *
 *  \author S Viret / L Storchi
 *  \date   2016, Mar 4
 *
 */

#ifndef TRACK_FITTER_PCA_H
#define TRACK_FITTER_PCA_H

#include "FWCore/Framework/interface/Frameworkfwd.h"
#include "FWCore/Framework/interface/EDProducer.h"
#include "FWCore/Framework/interface/Event.h"
#include "FWCore/Framework/interface/EventSetup.h"
#include "FWCore/Framework/interface/ESHandle.h"
#include "FWCore/Framework/interface/MakerMacros.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"

#include "DataFormats/L1TrackTrigger/interface/TTTypes.h"
#include "DataFormats/L1TrackTrigger/interface/TTCluster.h"
#include "DataFormats/L1TrackTrigger/interface/TTStub.h"
#include "DataFormats/L1TrackTrigger/interface/TTTrack.h"
#include "DataFormats/Common/interface/DetSetVectorNew.h"
#include "DataFormats/GeometryCommonDetAlgo/interface/MeasurementPoint.h"
#include "DataFormats/BeamSpot/interface/BeamSpot.h"
#include "MagneticField/Engine/interface/MagneticField.h"
#include "MagneticField/Records/interface/IdealMagneticFieldRecord.h"
#include "Geometry/TrackerGeometryBuilder/interface/TrackerGeometry.h"
#include "Geometry/Records/interface/TrackerDigiGeometryRecord.h"
#include "Geometry/CommonDetUnit/interface/GeomDetUnit.h"
#include "Geometry/Records/interface/StackedTrackerGeometryRecord.h"
#include "Geometry/TrackerGeometryBuilder/interface/StackedTrackerGeometry.h"

#include "L1Trigger/TrackFindingAM/interface/CMSPatternLayer.h"
#include "L1Trigger/TrackFindingAM/interface/PatternFinder.h"
#include "L1Trigger/TrackFindingAM/interface/SectorTree.h"
#include "L1Trigger/TrackFindingAM/interface/Hit.h"
#include "L1Trigger/TrackFindingAM/interface/PCATrackFitter.h"

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/shared_ptr.hpp>
#include <memory>
#include <string>
#include <map>
#include <vector>
#include <iostream>
#include <fstream>
#include <math.h>

//#ifndef __APPLE__
//BOOST_CLASS_EXPORT_IMPLEMENT(CMSPatternLayer)
//#endif

class TrackFitPCAProducer : public edm::EDProducer
{
  public:
    /// Constructor
    explicit TrackFitPCAProducer( const edm::ParameterSet& iConfig );

    /// Destructor;
    ~TrackFitPCAProducer();

  private:

  /// Data members
  double                       mMagneticField;
  unsigned int                 nSectors;
  unsigned int                 nWedges;
  std::string                  nBKName;
  int                          nThresh;
  const StackedTrackerGeometry *theStackedTracker;
  edm::InputTag                TTStubsInputTag;
  edm::InputTag                TTPatternsInputTag;
  std::string                  TTTrackOutputTag;
  std::string                  TTTrackBinaryOutputTag;

  /// Mandatory methods
  virtual void beginRun( const edm::Run& run, const edm::EventSetup& iSetup );
  virtual void endRun( const edm::Run& run, const edm::EventSetup& iSetup );
  virtual void produce( edm::Event& iEvent, const edm::EventSetup& iSetup );

}; /// Close class

/*! \brief   Implementation of methods
 */

/// Constructors
TrackFitPCAProducer::TrackFitPCAProducer( const edm::ParameterSet& iConfig )
{
  TTStubsInputTag          = iConfig.getParameter< edm::InputTag >( "TTInputStubs" );
  TTPatternsInputTag       = iConfig.getParameter< edm::InputTag >( "TTInputPatterns" );
  TTTrackOutputTag         = iConfig.getParameter< std::string >( "TTTrackName" );

  produces< std::vector< TTTrack< Ref_PixelDigi_ > > >( TTTrackOutputTag );
}

/// Destructor
TrackFitPCAProducer::~TrackFitPCAProducer() {}

/// Begin run
void TrackFitPCAProducer::beginRun( const edm::Run& run, const edm::EventSetup& iSetup )
{
  /// Get the geometry references
  edm::ESHandle< StackedTrackerGeometry > StackedTrackerGeomHandle;
  iSetup.get< StackedTrackerGeometryRecord >().get( StackedTrackerGeomHandle );
  theStackedTracker = StackedTrackerGeomHandle.product();

  /// Get magnetic field
  edm::ESHandle<MagneticField> magneticFieldHandle;
  iSetup.get<IdealMagneticFieldRecord>().get(magneticFieldHandle);
  const MagneticField* theMagneticField = magneticFieldHandle.product();
  double mMagneticFieldStrength = theMagneticField->inTesla(GlobalPoint(0,0,0)).z();
  mMagneticField = (floor(mMagneticFieldStrength*10.0 + 0.5))/10.0;
}

/// End run
void TrackFitPCAProducer::endRun( const edm::Run& run, const edm::EventSetup& iSetup ) {}

/// Implement the producer
void TrackFitPCAProducer::produce( edm::Event& iEvent, const edm::EventSetup& iSetup )
{
  /// Prepare output
  /// The temporary collection is used to store tracks
  /// before removal of duplicates
  std::auto_ptr< std::vector< TTTrack< Ref_PixelDigi_ > > > TTTracksForOutput( new std::vector< TTTrack< Ref_PixelDigi_ > > );

  /// Get the Stubs already stored away
  edm::Handle< edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > > > TTStubHandle;
  edm::Handle< std::vector< TTTrack< Ref_PixelDigi_ > > > TTPatternHandle;

  iEvent.getByLabel( TTStubsInputTag, TTStubHandle );
  iEvent.getByLabel( TTPatternsInputTag, TTPatternHandle );

  /// STEP 0
  /// Prepare output
  TTTracksForOutput->clear();

  int layer  = 0;
  int ladder = 0;
  int module = 0;

  int nbLayers = 0;

  /// Loop over TCs
  unsigned int tkCnt = 0;
  unsigned int j     = 0;

  std::map< unsigned int , edm::Ref< edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > >, TTStub< Ref_PixelDigi_ > > > stubMap;

  PCATrackFitter* PCA  = new PCATrackFitter(nbLayers); // Floating point

  /// STEP 0
  /// Read PCAConst file
  PCA->read_float_const_filename ("../data/barrel_tow16_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow17_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow18_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow19_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow20_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow21_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow22_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow23_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow24_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow25_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow26_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow27_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow28_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow29_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow30_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow31_pca_const.txt");

  /// STEP 1
  /// Loop over track candidates

  //  std::cout << "Start the loop over " << TTPatternHandle->size() << " pattern(s) in order to recover the stubs" << std::endl;

  std::vector<Hit*> m_hits;
  for(unsigned int i=0;i<m_hits.size();i++) delete m_hits[i];
  m_hits.clear();

  std::vector<Track*> tracks;
  for(unsigned int i=0;i<tracks.size();i++) delete tracks[i];
  tracks.clear();

  edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > >::const_iterator inputIter;
  edmNew::DetSet< TTStub< Ref_PixelDigi_ > >::const_iterator stubIter;

  std::vector< TTTrack< Ref_PixelDigi_ > >::const_iterator iterTTTrack;

  /// Go on only if there are TCs from PixelDigis
  if ( TTPatternHandle->size() > 0 )
  {
    for ( iterTTTrack = TTPatternHandle->begin();
	  iterTTTrack != TTPatternHandle->end();
	  ++iterTTTrack )
    {
      edm::Ptr< TTTrack< Ref_PixelDigi_ > > tempTrackPtr( TTPatternHandle, tkCnt++ );

      j = 0;

      m_hits.clear();
      tracks.clear();
      stubMap.clear();

      /// Get everything relevant
      unsigned int seedSector = tempTrackPtr->getSector();

      Track* TC = new Track();
      TC->setCharge(tempTrackPtr->getWedge());
      TC->setCurve(tempTrackPtr->getMomentum(5).perp());
      TC->setEta0(tempTrackPtr->getMomentum(5).eta());
      TC->setPhi0(tempTrackPtr->getMomentum(5).phi());
      TC->setZ0(tempTrackPtr->getPOCA(5).z());

      // Get the stubs in the TC

      std::vector< edm::Ref< edmNew::DetSetVector< TTStub< Ref_PixelDigi_  > >, TTStub< Ref_PixelDigi_  > > > trackStubs = tempTrackPtr->getStubRefs();

      // Loop over stubs contained in the pattern to recover the info

      for(unsigned int i=0;i<trackStubs.size();i++)
      {
	++j;

	edm::Ref< edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > >, TTStub< Ref_PixelDigi_ > > tempStubRef = trackStubs.at(i);

	stubMap.insert( std::make_pair( j, tempStubRef ) );

	/// Calculate average coordinates col/row for inner/outer Cluster
	/// These are already corrected for being at the center of each pixel
	MeasurementPoint mp0 = tempStubRef->getClusterRef(0)->findAverageLocalCoordinates();
	GlobalPoint posStub  = theStackedTracker->findGlobalPosition( &(*tempStubRef) );

	StackedTrackerDetId detIdStub( tempStubRef->getDetId() );
	//bool isPS = theStackedTracker->isPSModule( detIdStub );

	const GeomDetUnit* det0 = theStackedTracker->idToDetUnit( detIdStub, 0 );
	const GeomDetUnit* det1 = theStackedTracker->idToDetUnit( detIdStub, 1 );

	/// Find pixel pitch and topology related information
	const PixelGeomDetUnit* pix0 = dynamic_cast< const PixelGeomDetUnit* >( det0 );
	const PixelGeomDetUnit* pix1 = dynamic_cast< const PixelGeomDetUnit* >( det1 );
	const PixelTopology* top0    = dynamic_cast< const PixelTopology* >( &(pix0->specificTopology()) );
	const PixelTopology* top1    = dynamic_cast< const PixelTopology* >( &(pix1->specificTopology()) );

	/// Find the z-segment
	int cols0   = top0->ncolumns();
	int cols1   = top1->ncolumns();
	int ratio   = cols0/cols1; /// This assumes the ratio is integer!
	int segment = floor( mp0.y() / ratio );

	// Here we rearrange the number in order to be compatible with the AM emulator
	if ( detIdStub.isBarrel() )
	{
	  layer  = detIdStub.iLayer()+4;
	  ladder = detIdStub.iPhi()-1;
	  module = detIdStub.iZ()-1;
	}
	else if ( detIdStub.isEndcap() )
	{
	  layer  = 10+detIdStub.iZ()+abs((int)(detIdStub.iSide())-2)*7;
	  ladder = detIdStub.iRing()-1;
	  module = detIdStub.iPhi()-1;
	}

	int strip  =  mp0.x();
	int tp     = -1;
	float eta  = 0;
	float phi0 = 0;
	float spt  = 0;
	float x    = posStub.x();
	float y    = posStub.y();
	float z    = posStub.z();
	float x0   = 0.;
	float y0   = 0.;
	float z0   = 0.;
	float ip   = sqrt(x0*x0+y0*y0);

	Hit* h = new Hit(layer,ladder, module, segment, strip,
			 j, tp, spt, ip, eta, phi0, x, y, z, x0, y0, z0, tempStubRef->getTriggerDisplacement()-tempStubRef->getTriggerOffset());
	m_hits.push_back(h);

      } /// End of loop over track stubs

      if (tempTrackPtr->getSector()!=18) continue; // For the moment don't need to bother with the rest

      PCA->setSectorID(tempTrackPtr->getSector());
      PCA->setTrack(TC);
      PCA->fit(m_hits);
      tracks = PCA->getTracks();
      PCA->clean();

      delete TC;

      std::vector<double> chi2v = PCA->get_chi2f();

      if (chi2v.size() == tracks.size())
      {
        // Store the tracks (no duplicate cleaning yet)
        //      cout<<"Found "<<tracks.size()<<" track"<<endl;

        std::vector< edm::Ref< edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > >, TTStub< Ref_PixelDigi_ > > > tempVec;

        for(unsigned int tt=0;tt<tracks.size();tt++)
        {
          tempVec.clear();

          vector<int> stubs = tracks[tt]->getStubs();
          for(unsigned int sti=0;sti<stubs.size();sti++) tempVec.push_back( stubMap[ stubs[sti] ]);

          double pz = tracks[tt]->getCurve()/(tan(2*atan(exp(-tracks[tt]->getEta0()))));

          TTTrack< Ref_PixelDigi_ > tempTrack( tempVec );
          GlobalPoint POCA(0.,0.,tracks[tt]->getZ0());
          GlobalVector mom(tracks[tt]->getCurve()*cos(tracks[tt]->getPhi0()),
          		 tracks[tt]->getCurve()*sin(tracks[tt]->getPhi0()),
          		 pz);

          tempTrack.setSector( seedSector );
          tempTrack.setWedge( tracks[tt]->getCharge() );
          tempTrack.setMomentum( mom , 5);
          tempTrack.setPOCA( POCA , 5);
          tempTrack.setChi2(chi2v[tt]);

          TTTracksForOutput->push_back( tempTrack );

          delete tracks[tt];
        }
      }
    } // End of loop over patterns

    delete(PCA);

  }

  /// Put in the event content
  iEvent.put( TTTracksForOutput, TTTrackOutputTag);
}

// DEFINE THIS AS A PLUG-IN
DEFINE_FWK_MODULE(TrackFitPCAProducer);

#endif

<commit_msg>bug fix to have all barrel sectors in fit<commit_after>/*! \class   TrackFitPCAProducer
 *
 *  \author S Viret / L Storchi
 *  \date   2016, Mar 4
 *
 */

#ifndef TRACK_FITTER_PCA_H
#define TRACK_FITTER_PCA_H

#include "FWCore/Framework/interface/Frameworkfwd.h"
#include "FWCore/Framework/interface/EDProducer.h"
#include "FWCore/Framework/interface/Event.h"
#include "FWCore/Framework/interface/EventSetup.h"
#include "FWCore/Framework/interface/ESHandle.h"
#include "FWCore/Framework/interface/MakerMacros.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"

#include "DataFormats/L1TrackTrigger/interface/TTTypes.h"
#include "DataFormats/L1TrackTrigger/interface/TTCluster.h"
#include "DataFormats/L1TrackTrigger/interface/TTStub.h"
#include "DataFormats/L1TrackTrigger/interface/TTTrack.h"
#include "DataFormats/Common/interface/DetSetVectorNew.h"
#include "DataFormats/GeometryCommonDetAlgo/interface/MeasurementPoint.h"
#include "DataFormats/BeamSpot/interface/BeamSpot.h"
#include "MagneticField/Engine/interface/MagneticField.h"
#include "MagneticField/Records/interface/IdealMagneticFieldRecord.h"
#include "Geometry/TrackerGeometryBuilder/interface/TrackerGeometry.h"
#include "Geometry/Records/interface/TrackerDigiGeometryRecord.h"
#include "Geometry/CommonDetUnit/interface/GeomDetUnit.h"
#include "Geometry/Records/interface/StackedTrackerGeometryRecord.h"
#include "Geometry/TrackerGeometryBuilder/interface/StackedTrackerGeometry.h"

#include "L1Trigger/TrackFindingAM/interface/CMSPatternLayer.h"
#include "L1Trigger/TrackFindingAM/interface/PatternFinder.h"
#include "L1Trigger/TrackFindingAM/interface/SectorTree.h"
#include "L1Trigger/TrackFindingAM/interface/Hit.h"
#include "L1Trigger/TrackFindingAM/interface/PCATrackFitter.h"

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/shared_ptr.hpp>
#include <memory>
#include <string>
#include <map>
#include <vector>
#include <iostream>
#include <fstream>
#include <math.h>

//#ifndef __APPLE__
//BOOST_CLASS_EXPORT_IMPLEMENT(CMSPatternLayer)
//#endif

class TrackFitPCAProducer : public edm::EDProducer
{
  public:
    /// Constructor
    explicit TrackFitPCAProducer( const edm::ParameterSet& iConfig );

    /// Destructor;
    ~TrackFitPCAProducer();

  private:

  /// Data members
  double                       mMagneticField;
  unsigned int                 nSectors;
  unsigned int                 nWedges;
  std::string                  nBKName;
  int                          nThresh;
  const StackedTrackerGeometry *theStackedTracker;
  edm::InputTag                TTStubsInputTag;
  edm::InputTag                TTPatternsInputTag;
  std::string                  TTTrackOutputTag;
  std::string                  TTTrackBinaryOutputTag;

  /// Mandatory methods
  virtual void beginRun( const edm::Run& run, const edm::EventSetup& iSetup );
  virtual void endRun( const edm::Run& run, const edm::EventSetup& iSetup );
  virtual void produce( edm::Event& iEvent, const edm::EventSetup& iSetup );

}; /// Close class

/*! \brief   Implementation of methods
 */

/// Constructors
TrackFitPCAProducer::TrackFitPCAProducer( const edm::ParameterSet& iConfig )
{
  TTStubsInputTag          = iConfig.getParameter< edm::InputTag >( "TTInputStubs" );
  TTPatternsInputTag       = iConfig.getParameter< edm::InputTag >( "TTInputPatterns" );
  TTTrackOutputTag         = iConfig.getParameter< std::string >( "TTTrackName" );

  produces< std::vector< TTTrack< Ref_PixelDigi_ > > >( TTTrackOutputTag );
}

/// Destructor
TrackFitPCAProducer::~TrackFitPCAProducer() {}

/// Begin run
void TrackFitPCAProducer::beginRun( const edm::Run& run, const edm::EventSetup& iSetup )
{
  /// Get the geometry references
  edm::ESHandle< StackedTrackerGeometry > StackedTrackerGeomHandle;
  iSetup.get< StackedTrackerGeometryRecord >().get( StackedTrackerGeomHandle );
  theStackedTracker = StackedTrackerGeomHandle.product();

  /// Get magnetic field
  edm::ESHandle<MagneticField> magneticFieldHandle;
  iSetup.get<IdealMagneticFieldRecord>().get(magneticFieldHandle);
  const MagneticField* theMagneticField = magneticFieldHandle.product();
  double mMagneticFieldStrength = theMagneticField->inTesla(GlobalPoint(0,0,0)).z();
  mMagneticField = (floor(mMagneticFieldStrength*10.0 + 0.5))/10.0;
}

/// End run
void TrackFitPCAProducer::endRun( const edm::Run& run, const edm::EventSetup& iSetup ) {}

/// Implement the producer
void TrackFitPCAProducer::produce( edm::Event& iEvent, const edm::EventSetup& iSetup )
{
  /// Prepare output
  /// The temporary collection is used to store tracks
  /// before removal of duplicates
  std::auto_ptr< std::vector< TTTrack< Ref_PixelDigi_ > > > TTTracksForOutput( new std::vector< TTTrack< Ref_PixelDigi_ > > );

  /// Get the Stubs already stored away
  edm::Handle< edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > > > TTStubHandle;
  edm::Handle< std::vector< TTTrack< Ref_PixelDigi_ > > > TTPatternHandle;

  iEvent.getByLabel( TTStubsInputTag, TTStubHandle );
  iEvent.getByLabel( TTPatternsInputTag, TTPatternHandle );

  /// STEP 0
  /// Prepare output
  TTTracksForOutput->clear();

  int layer  = 0;
  int ladder = 0;
  int module = 0;

  int nbLayers = 0;

  /// Loop over TCs
  unsigned int tkCnt = 0;
  unsigned int j     = 0;

  std::map< unsigned int , edm::Ref< edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > >, TTStub< Ref_PixelDigi_ > > > stubMap;

  PCATrackFitter* PCA  = new PCATrackFitter(nbLayers); // Floating point

  /// STEP 0
  /// Read PCAConst file
  PCA->read_float_const_filename ("../data/barrel_tow16_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow17_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow18_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow19_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow20_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow21_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow22_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow23_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow24_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow25_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow26_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow27_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow28_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow29_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow30_pca_const.txt");
  PCA->read_float_const_filename ("../data/barrel_tow31_pca_const.txt");

  /// STEP 1
  /// Loop over track candidates

  //  std::cout << "Start the loop over " << TTPatternHandle->size() << " pattern(s) in order to recover the stubs" << std::endl;

  std::vector<Hit*> m_hits;
  for(unsigned int i=0;i<m_hits.size();i++) delete m_hits[i];
  m_hits.clear();

  std::vector<Track*> tracks;
  for(unsigned int i=0;i<tracks.size();i++) delete tracks[i];
  tracks.clear();

  edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > >::const_iterator inputIter;
  edmNew::DetSet< TTStub< Ref_PixelDigi_ > >::const_iterator stubIter;

  std::vector< TTTrack< Ref_PixelDigi_ > >::const_iterator iterTTTrack;

  /// Go on only if there are TCs from PixelDigis
  if ( TTPatternHandle->size() > 0 )
  {
    for ( iterTTTrack = TTPatternHandle->begin();
	  iterTTTrack != TTPatternHandle->end();
	  ++iterTTTrack )
    {
      edm::Ptr< TTTrack< Ref_PixelDigi_ > > tempTrackPtr( TTPatternHandle, tkCnt++ );

      j = 0;

      m_hits.clear();
      tracks.clear();
      stubMap.clear();

      /// Get everything relevant
      unsigned int seedSector = tempTrackPtr->getSector();

      Track* TC = new Track();
      TC->setCharge(tempTrackPtr->getWedge());
      TC->setCurve(tempTrackPtr->getMomentum(5).perp());
      TC->setEta0(tempTrackPtr->getMomentum(5).eta());
      TC->setPhi0(tempTrackPtr->getMomentum(5).phi());
      TC->setZ0(tempTrackPtr->getPOCA(5).z());

      // Get the stubs in the TC

      std::vector< edm::Ref< edmNew::DetSetVector< TTStub< Ref_PixelDigi_  > >, TTStub< Ref_PixelDigi_  > > > trackStubs = tempTrackPtr->getStubRefs();

      // Loop over stubs contained in the pattern to recover the info

      for(unsigned int i=0;i<trackStubs.size();i++)
      {
	++j;

	edm::Ref< edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > >, TTStub< Ref_PixelDigi_ > > tempStubRef = trackStubs.at(i);

	stubMap.insert( std::make_pair( j, tempStubRef ) );

	/// Calculate average coordinates col/row for inner/outer Cluster
	/// These are already corrected for being at the center of each pixel
	MeasurementPoint mp0 = tempStubRef->getClusterRef(0)->findAverageLocalCoordinates();
	GlobalPoint posStub  = theStackedTracker->findGlobalPosition( &(*tempStubRef) );

	StackedTrackerDetId detIdStub( tempStubRef->getDetId() );
	//bool isPS = theStackedTracker->isPSModule( detIdStub );

	const GeomDetUnit* det0 = theStackedTracker->idToDetUnit( detIdStub, 0 );
	const GeomDetUnit* det1 = theStackedTracker->idToDetUnit( detIdStub, 1 );

	/// Find pixel pitch and topology related information
	const PixelGeomDetUnit* pix0 = dynamic_cast< const PixelGeomDetUnit* >( det0 );
	const PixelGeomDetUnit* pix1 = dynamic_cast< const PixelGeomDetUnit* >( det1 );
	const PixelTopology* top0    = dynamic_cast< const PixelTopology* >( &(pix0->specificTopology()) );
	const PixelTopology* top1    = dynamic_cast< const PixelTopology* >( &(pix1->specificTopology()) );

	/// Find the z-segment
	int cols0   = top0->ncolumns();
	int cols1   = top1->ncolumns();
	int ratio   = cols0/cols1; /// This assumes the ratio is integer!
	int segment = floor( mp0.y() / ratio );

	// Here we rearrange the number in order to be compatible with the AM emulator
	if ( detIdStub.isBarrel() )
	{
	  layer  = detIdStub.iLayer()+4;
	  ladder = detIdStub.iPhi()-1;
	  module = detIdStub.iZ()-1;
	}
	else if ( detIdStub.isEndcap() )
	{
	  layer  = 10+detIdStub.iZ()+abs((int)(detIdStub.iSide())-2)*7;
	  ladder = detIdStub.iRing()-1;
	  module = detIdStub.iPhi()-1;
	}

	int strip  =  mp0.x();
	int tp     = -1;
	float eta  = 0;
	float phi0 = 0;
	float spt  = 0;
	float x    = posStub.x();
	float y    = posStub.y();
	float z    = posStub.z();
	float x0   = 0.;
	float y0   = 0.;
	float z0   = 0.;
	float ip   = sqrt(x0*x0+y0*y0);

	Hit* h = new Hit(layer,ladder, module, segment, strip,
			 j, tp, spt, ip, eta, phi0, x, y, z, x0, y0, z0, tempStubRef->getTriggerDisplacement()-tempStubRef->getTriggerOffset());
	m_hits.push_back(h);

      } /// End of loop over track stubs

      //if (tempTrackPtr->getSector()!=18) continue; // For the moment don't need to bother with the rest
      if (tempTrackPtr->getSector() < 16 || tempTrackPtr->getSector() > 31) continue; // For the moment don't need to bother with the rest

      PCA->setSectorID(tempTrackPtr->getSector());
      PCA->setTrack(TC);
      PCA->fit(m_hits);
      tracks = PCA->getTracks();
      PCA->clean();

      delete TC;

      std::vector<double> chi2v = PCA->get_chi2f();

      if (chi2v.size() == tracks.size())
      {
        // Store the tracks (no duplicate cleaning yet)
        //      cout<<"Found "<<tracks.size()<<" track"<<endl;

        std::vector< edm::Ref< edmNew::DetSetVector< TTStub< Ref_PixelDigi_ > >, TTStub< Ref_PixelDigi_ > > > tempVec;

        for(unsigned int tt=0;tt<tracks.size();tt++)
        {
          tempVec.clear();

          vector<int> stubs = tracks[tt]->getStubs();
          for(unsigned int sti=0;sti<stubs.size();sti++) tempVec.push_back( stubMap[ stubs[sti] ]);

          double pz = tracks[tt]->getCurve()/(tan(2*atan(exp(-tracks[tt]->getEta0()))));

          TTTrack< Ref_PixelDigi_ > tempTrack( tempVec );
          GlobalPoint POCA(0.,0.,tracks[tt]->getZ0());
          GlobalVector mom(tracks[tt]->getCurve()*cos(tracks[tt]->getPhi0()),
          		 tracks[tt]->getCurve()*sin(tracks[tt]->getPhi0()),
          		 pz);

          tempTrack.setSector( seedSector );
          tempTrack.setWedge( tracks[tt]->getCharge() );
          tempTrack.setMomentum( mom , 5);
          tempTrack.setPOCA( POCA , 5);
          tempTrack.setChi2(chi2v[tt]);

          TTTracksForOutput->push_back( tempTrack );

          delete tracks[tt];
        }
      }
    } // End of loop over patterns

    delete(PCA);

  }

  /// Put in the event content
  iEvent.put( TTTracksForOutput, TTTrackOutputTag);
}

// DEFINE THIS AS A PLUG-IN
DEFINE_FWK_MODULE(TrackFitPCAProducer);

#endif

<|endoftext|>


<commit_before>/*
 * The Apache Software License, Version 1.1
 *
 * Copyright (c) 1999 The Apache Software Foundation.  All rights
 * reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * 3. The end-user documentation included with the redistribution,
 *    if any, must include the following acknowledgment:
 *       "This product includes software developed by the
 *        Apache Software Foundation (http://www.apache.org/)."
 *    Alternately, this acknowledgment may appear in the software itself,
 *    if and wherever such third-party acknowledgments normally appear.
 *
 * 4. The names "Xerces" and "Apache Software Foundation" must
 *    not be used to endorse or promote products derived from this
 *    software without prior written permission. For written
 *    permission, please contact apache\@apache.org.
 *
 * 5. Products derived from this software may not be called "Apache",
 *    nor may "Apache" appear in their name, without prior written
 *    permission of the Apache Software Foundation.
 *
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESSED OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE APACHE SOFTWARE FOUNDATION OR
 * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 * ====================================================================
 *
 * This software consists of voluntary contributions made by many
 * individuals on behalf of the Apache Software Foundation, and was
 * originally based on software copyright (c) 1999, International
 * Business Machines, Inc., http://www.ibm.com .  For more information
 * on the Apache Software Foundation, please see
 * <http://www.apache.org/>.
 */

/**
 * $Log$
 * Revision 1.1  1999/11/09 01:05:35  twl
 * Initial revision
 *
 * Revision 1.3  1999/11/08 20:45:19  rahul
 * Swat for adding in Product name and CVS comment log variable.
 *
 */

#if !defined(XML4CDEFS_HPP)
#define XML4CDEFS_HPP

// ---------------------------------------------------------------------------
//  These are the various representations of the current version of XML4C.
//  These are updated for every build. They must be at the top because they
//  can be used by various per-compiler headers below.
// ---------------------------------------------------------------------------
#define XML4C_DLLVersionStr "3_0"
static const char* const    gXML4CVersionStr = "3_0";
static const char* const    gXML4CFullVersionStr = "3_0_0";
static const unsigned int   gXML4CMajVersion = 3;
static const unsigned int   gXML4CMinVersion = 0;
static const unsigned int   gXML4CRevision   = 0;

// ---------------------------------------------------------------------------
//  Include the header that does automatic sensing of the current platform
//  and compiler.
// ---------------------------------------------------------------------------
#include    <util/AutoSense.hpp>

// ---------------------------------------------------------------------------
//  According to the platform we include a platform specific file. This guy
//  will set up any platform specific stuff, such as character mode.
// ---------------------------------------------------------------------------
#if defined(XML_WIN32)
#include    <util/Platforms/Win32/Win32Defs.hpp>
#endif

#if defined(XML_AIX)
#include    <util/Platforms/AIX/AIXDefs.hpp>
#endif

#if defined(XML_SOLARIS)
#include    <util/Platforms/Solaris/SolarisDefs.hpp>
#endif

#if defined(XML_HPUX)
#include    <util/Platforms/HPUX/HPUXDefs.hpp>
#endif

#if defined(XML_TANDEM)
#include    <util/Platforms/Tandem/TandemDefs.hpp>
#endif

#if defined(XML_LINUX)
#include    <util/Platforms/Linux/LinuxDefs.hpp>
#endif

#if defined(XML_OE390)
#include    <util/Platforms/OS390/OE390Defs.hpp>
#endif

#if defined(XML_OS2)
#include    <util/Platforms/OS2/OS2Defs.hpp>
#endif

#if defined(XML_MACOS)
#include	<util/Platforms/MaxOS/MacOSDefs.hpp>
#endif

// ---------------------------------------------------------------------------
//  And now we subinclude a header according to the development environment
//  we are on. This guy defines for each platform some basic stuff that is
//  specific to the development environment.
// ---------------------------------------------------------------------------
#if defined(XML_VISUALCPP)
#include    <util/Compilers/VCPPDefs.hpp>
#endif

#if defined(XML_CSET)
#include    <util/Compilers/CSetDefs.hpp>
#endif

#if defined(XML_BORLAND)
#include    <util/Compilers/BorlandCDefs.hpp>
#endif

#if defined(XML_SUNCC)
#include    <util/Compilers/SunCCDefs.hpp>
#endif

#if defined(XML_SOLARIS_KAICC)
#include    <util/Compilers/SunKaiDefs.hpp>
#endif

#if defined(XML_GNUG)
#include    <util/Compilers/GNUGDefs.hpp>
#endif

#if defined(XML_HPUX_CC) || defined(XML_HPUX_aCC) || defined(XML_HPUX_KAICC)
#include    <util/Compilers/HPCCDefs.hpp>
#endif

#if defined(XML_TANDEMCC)
#include    <util/Compilers/TandemCCDefs.hpp>
#endif

#if defined(XML_GCC)
#include    <util/Compilers/GCCDefs.hpp>
#endif

#if defined(XML_MVSCPP)
#include    <util/Compilers/MVSCPPDefs.hpp>
#endif

#if defined(XML_IBMVAW32)
#include    <util/Compilers/IBMVAW32Defs.hpp>
#endif

#if defined(XML_IBMVAOS2)
#include    <util/Compilers/IBMVAOS2Defs.hpp>
#endif

#if defined(XML_METROWERKS)
#include	<util/Compilers/CodeWarriorDefs.hpp>
#endif

// ---------------------------------------------------------------------------
//  Some general typedefs that are defined for internal flexibility.
//
//  Note  that UTF16Ch is fixed at 16 bits, whereas XMLCh floats in size per
//  platform, to whatever is the native wide char format there. UCS4Ch is
//  fixed at 32 bits. The types we defined them in terms of are defined per
//  compiler, using whatever types are the right ones for them to get these
//  16/32 bit sizes.
// ---------------------------------------------------------------------------
typedef unsigned char       XMLByte;
typedef XMLUInt16           UTF16Ch;
typedef XMLUInt32           UCS4Ch;

// ---------------------------------------------------------------------------
//  Handle boolean. If the platform can handle booleans itself, then we
//  map our boolean type to the native type. Otherwise we create a default
//  one as an int and define const values for true and false.
//
//  This flag will be set in the per-development environment stuff above.
// ---------------------------------------------------------------------------
#if defined(NO_NATIVE_BOOL)
typedef int     bool;
const   bool    true    = 1;
const   bool    false   = 0;
#endif

// ---------------------------------------------------------------------------
//  Set up the import/export keyword  for our core projects. The
//  PLATFORM_XXXX keywords are set in the per-development environment
//  include above.
// ---------------------------------------------------------------------------
#if defined(PROJ_XMLUTIL)
#define XMLUTIL_EXPORT PLATFORM_EXPORT
#else
#define XMLUTIL_EXPORT PLATFORM_IMPORT
#endif

#if defined(PROJ_XMLPARSER)
#define XMLPARSER_EXPORT PLATFORM_EXPORT
#else
#define XMLPARSER_EXPORT PLATFORM_IMPORT
#endif

#if defined(PROJ_SAX4C)
#define SAX_EXPORT PLATFORM_EXPORT
#else
#define SAX_EXPORT PLATFORM_IMPORT
#endif

#if defined(PROJ_DOM)
#define CDOM_EXPORT PLATFORM_EXPORT
#else
#define CDOM_EXPORT PLATFORM_IMPORT
#endif

#if defined(PROJ_PARSERS)
#define PARSERS_EXPORT  PLATFORM_EXPORT
#else
#define PARSERS_EXPORT  PLATFORM_IMPORT
#endif

#if defined(PROJ_VALIDATORS)
#define VALIDATORS_EXPORT  PLATFORM_EXPORT
#else
#define VALIDATORS_EXPORT  PLATFORM_IMPORT
#endif

#endif
<commit_msg>Changed version numbers<commit_after>/*
 * The Apache Software License, Version 1.1
 *
 * Copyright (c) 1999 The Apache Software Foundation.  All rights
 * reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * 3. The end-user documentation included with the redistribution,
 *    if any, must include the following acknowledgment:
 *       "This product includes software developed by the
 *        Apache Software Foundation (http://www.apache.org/)."
 *    Alternately, this acknowledgment may appear in the software itself,
 *    if and wherever such third-party acknowledgments normally appear.
 *
 * 4. The names "Xerces" and "Apache Software Foundation" must
 *    not be used to endorse or promote products derived from this
 *    software without prior written permission. For written
 *    permission, please contact apache\@apache.org.
 *
 * 5. Products derived from this software may not be called "Apache",
 *    nor may "Apache" appear in their name, without prior written
 *    permission of the Apache Software Foundation.
 *
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESSED OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE APACHE SOFTWARE FOUNDATION OR
 * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 * ====================================================================
 *
 * This software consists of voluntary contributions made by many
 * individuals on behalf of the Apache Software Foundation, and was
 * originally based on software copyright (c) 1999, International
 * Business Machines, Inc., http://www.ibm.com .  For more information
 * on the Apache Software Foundation, please see
 * <http://www.apache.org/>.
 */

/**
 * $Log$
 * Revision 1.2  1999/11/10 02:02:51  abagchi
 * Changed version numbers
 *
 * Revision 1.1.1.1  1999/11/09 01:05:35  twl
 * Initial checkin
 *
 * Revision 1.3  1999/11/08 20:45:19  rahul
 * Swat for adding in Product name and CVS comment log variable.
 *
 */

#if !defined(XML4CDEFS_HPP)
#define XML4CDEFS_HPP

// ---------------------------------------------------------------------------
//  These are the various representations of the current version of XML4C.
//  These are updated for every build. They must be at the top because they
//  can be used by various per-compiler headers below.
// ---------------------------------------------------------------------------
#define XML4C_DLLVersionStr "1_0"
static const char* const    gXML4CVersionStr = "1_0";
static const char* const    gXML4CFullVersionStr = "1_0_0";
static const unsigned int   gXML4CMajVersion = 1;
static const unsigned int   gXML4CMinVersion = 0;
static const unsigned int   gXML4CRevision   = 0;

// ---------------------------------------------------------------------------
//  Include the header that does automatic sensing of the current platform
//  and compiler.
// ---------------------------------------------------------------------------
#include    <util/AutoSense.hpp>

// ---------------------------------------------------------------------------
//  According to the platform we include a platform specific file. This guy
//  will set up any platform specific stuff, such as character mode.
// ---------------------------------------------------------------------------
#if defined(XML_WIN32)
#include    <util/Platforms/Win32/Win32Defs.hpp>
#endif

#if defined(XML_AIX)
#include    <util/Platforms/AIX/AIXDefs.hpp>
#endif

#if defined(XML_SOLARIS)
#include    <util/Platforms/Solaris/SolarisDefs.hpp>
#endif

#if defined(XML_HPUX)
#include    <util/Platforms/HPUX/HPUXDefs.hpp>
#endif

#if defined(XML_TANDEM)
#include    <util/Platforms/Tandem/TandemDefs.hpp>
#endif

#if defined(XML_LINUX)
#include    <util/Platforms/Linux/LinuxDefs.hpp>
#endif

#if defined(XML_OE390)
#include    <util/Platforms/OS390/OE390Defs.hpp>
#endif

#if defined(XML_OS2)
#include    <util/Platforms/OS2/OS2Defs.hpp>
#endif

#if defined(XML_MACOS)
#include	<util/Platforms/MaxOS/MacOSDefs.hpp>
#endif

// ---------------------------------------------------------------------------
//  And now we subinclude a header according to the development environment
//  we are on. This guy defines for each platform some basic stuff that is
//  specific to the development environment.
// ---------------------------------------------------------------------------
#if defined(XML_VISUALCPP)
#include    <util/Compilers/VCPPDefs.hpp>
#endif

#if defined(XML_CSET)
#include    <util/Compilers/CSetDefs.hpp>
#endif

#if defined(XML_BORLAND)
#include    <util/Compilers/BorlandCDefs.hpp>
#endif

#if defined(XML_SUNCC)
#include    <util/Compilers/SunCCDefs.hpp>
#endif

#if defined(XML_SOLARIS_KAICC)
#include    <util/Compilers/SunKaiDefs.hpp>
#endif

#if defined(XML_GNUG)
#include    <util/Compilers/GNUGDefs.hpp>
#endif

#if defined(XML_HPUX_CC) || defined(XML_HPUX_aCC) || defined(XML_HPUX_KAICC)
#include    <util/Compilers/HPCCDefs.hpp>
#endif

#if defined(XML_TANDEMCC)
#include    <util/Compilers/TandemCCDefs.hpp>
#endif

#if defined(XML_GCC)
#include    <util/Compilers/GCCDefs.hpp>
#endif

#if defined(XML_MVSCPP)
#include    <util/Compilers/MVSCPPDefs.hpp>
#endif

#if defined(XML_IBMVAW32)
#include    <util/Compilers/IBMVAW32Defs.hpp>
#endif

#if defined(XML_IBMVAOS2)
#include    <util/Compilers/IBMVAOS2Defs.hpp>
#endif

#if defined(XML_METROWERKS)
#include	<util/Compilers/CodeWarriorDefs.hpp>
#endif

// ---------------------------------------------------------------------------
//  Some general typedefs that are defined for internal flexibility.
//
//  Note  that UTF16Ch is fixed at 16 bits, whereas XMLCh floats in size per
//  platform, to whatever is the native wide char format there. UCS4Ch is
//  fixed at 32 bits. The types we defined them in terms of are defined per
//  compiler, using whatever types are the right ones for them to get these
//  16/32 bit sizes.
// ---------------------------------------------------------------------------
typedef unsigned char       XMLByte;
typedef XMLUInt16           UTF16Ch;
typedef XMLUInt32           UCS4Ch;

// ---------------------------------------------------------------------------
//  Handle boolean. If the platform can handle booleans itself, then we
//  map our boolean type to the native type. Otherwise we create a default
//  one as an int and define const values for true and false.
//
//  This flag will be set in the per-development environment stuff above.
// ---------------------------------------------------------------------------
#if defined(NO_NATIVE_BOOL)
typedef int     bool;
const   bool    true    = 1;
const   bool    false   = 0;
#endif

// ---------------------------------------------------------------------------
//  Set up the import/export keyword  for our core projects. The
//  PLATFORM_XXXX keywords are set in the per-development environment
//  include above.
// ---------------------------------------------------------------------------
#if defined(PROJ_XMLUTIL)
#define XMLUTIL_EXPORT PLATFORM_EXPORT
#else
#define XMLUTIL_EXPORT PLATFORM_IMPORT
#endif

#if defined(PROJ_XMLPARSER)
#define XMLPARSER_EXPORT PLATFORM_EXPORT
#else
#define XMLPARSER_EXPORT PLATFORM_IMPORT
#endif

#if defined(PROJ_SAX4C)
#define SAX_EXPORT PLATFORM_EXPORT
#else
#define SAX_EXPORT PLATFORM_IMPORT
#endif

#if defined(PROJ_DOM)
#define CDOM_EXPORT PLATFORM_EXPORT
#else
#define CDOM_EXPORT PLATFORM_IMPORT
#endif

#if defined(PROJ_PARSERS)
#define PARSERS_EXPORT  PLATFORM_EXPORT
#else
#define PARSERS_EXPORT  PLATFORM_IMPORT
#endif

#if defined(PROJ_VALIDATORS)
#define VALIDATORS_EXPORT  PLATFORM_EXPORT
#else
#define VALIDATORS_EXPORT  PLATFORM_IMPORT
#endif

#endif
<|endoftext|>


<commit_before>#include "HuboMotor.h"
#include <string>
#include <iostream>

HuboMotor::HuboMotor(){
    this->ticks_position = 0;
    this->desired_position = 0;
    this->omega = MAX_ANGULAR_VELOCITY;

    //NEW_DATA
    currGoal = 0;
	interStep = 0;
	interVel = 0;

	currVel = 0;
	currPos = 0;
	currCurrent = 0;
	currTemp = 0;

	enabled = false;
	homed = false;
	zeroed = false;
}

HuboMotor::HuboMotor(long mpos1, long mpos2, long kp, long kd, long ki,
                     long dz, long off, long hlim, long hld, long hv1,
                     long hv2, long hma, long sm, long ers, long as, long md,
                     long v_max, long a_max, long jam_lim, long jamd,
                     long pwm_lim, long i_err, long b_err){
    this->mpos1 = mpos1;
    this->mpos2 = mpos2;
    this->Kp = kp;
    this->Kd = kd;
    this->Ki = ki;
    this->dz = dz;
    this->off = off;
    this->hlim = hlim;
    this->hld = hld;
    this->hv1 = hv1;
    this->hv2 = hv2;
    this->hma = hma;
    this->sm = sm;
    this->ers = ers;
    this->as = as;
    this->md = md;
    this->v_max = v_max;
    this->a_max = a_max;
    this->jam_lim = jam_lim;
    this->jamd = jamd;
    this->pwm_lim = pwm_lim;
    this->i_err = i_err;
    this->b_err = b_err;
    this->ticks_position = 0;
    this->desired_position = 0;
    this->omega = MAX_ANGULAR_VELOCITY;

    currGoal = 0;
	interStep = 0;
	interVel = 0;

	currVel = 0;
	currPos = 0;
	currCurrent = 0;
	currTemp = 0;

	enabled = false;
	homed = false;
	zeroed = false;
}

HuboMotor::HuboMotor(const HuboMotor& rhs){
    this->mpos1 = rhs.mpos1;
    this->mpos2 = rhs.mpos2;
    this->Kp = rhs.Kp;
    this->Kd = rhs.Kd;
    this->Ki = rhs.Ki;
    this->dz = rhs.dz;
    this->off = rhs.off;
    this->hlim = rhs.hlim;
    this->hld = rhs.hld;
    this->hv1 = rhs.hv1;
    this->hv2 = rhs.hv2;
    this->hma = rhs.hma;
    this->sm = rhs.sm;
    this->ers = rhs.ers;
    this->as = rhs.as;
    this->md = rhs.md;
    this->v_max = rhs.v_max;
    this->a_max = rhs.a_max;
    this->jam_lim = rhs.jam_lim;
    this->jamd = rhs.jamd;
    this->pwm_lim = rhs.pwm_lim;
    this->i_err = rhs.i_err;
    this->b_err = rhs.b_err;
    this->ticks_position = rhs.ticks_position;
    this->desired_position = rhs.desired_position;
    this->omega = rhs.omega;

    //NEW_DATA
    this->name = rhs.name;
    this->currGoal = rhs.currGoal;
    this->interStep = rhs.interStep;
    this->interVel = rhs.interVel;

    this->currVel = rhs.currVel;
    this->currPos = rhs.currPos;
    this->currCurrent = rhs.currCurrent;
    this->currTemp = rhs.currTemp;

    this->enabled = rhs.enabled;
    this->homed = rhs.homed;
    this->zeroed = rhs.zeroed;
}

void HuboMotor::setUpperLimit(long limit){
    this->mpos2 = limit;
}

void HuboMotor::setLowerLimit(long limit){
    this->mpos1 = limit;
}

void HuboMotor::setPositionGain(long kp, long kd, long ki){
    this->Kp = kp;
    this->Kd = kd;
    this->Ki = ki;
}

void HuboMotor::setCurrentGain(long kpt, long kdt, long kf){
    this->Kpt = kpt;
    this->Kdt = kdt;
    this->Kf = kf;
}

void HuboMotor::setDeadZone(int dz){
    this->dz = dz;
}

void HuboMotor::setHomeset1(long off, long hlim, long hld){
    this->off = off;
    this->hlim = hlim;
    this->hld = hld;
}

void HuboMotor::setHomeset2(long hv1, long hv2, long hma, long sm){
    this->hv1 = hv1;
    this->hv2 = hv2;
    this->hma = hma;
    this->sm = sm;
}

void HuboMotor::setEncoderResolution(long ers, long as, long md){
    this->ers = ers;
    this->as = as;
    this->md = md;
}

void HuboMotor::setSpeedLimit(long vel, long acc){
    this->v_max = vel;
    this->a_max = acc;
}

void HuboMotor::setJamPowerLimit(long jam_lim, long jamd, long pwm_lim){
    this->jam_lim = jam_lim;
    this->jamd = jamd;
    this->pwm_lim = pwm_lim;
}

void HuboMotor::setErrorLimit(long i_err, long b_err){
    this->i_err = i_err;
    this->b_err = b_err;
}

void HuboMotor::setGearRatios(long drive, long driven, long harm, long enc){
	this->drive = drive;
	this->driven = driven;
	this->harm = harm;
	this->enc = enc;
}

void HuboMotor::setTicksPosition(long ticks){
    this->ticks_position = ticks;
}

void HuboMotor::setDesiredPosition(long ticks){
	this->desired_position = ticks;
}

void HuboMotor::setAngularVelocity(double omega){
	this->omega = omega;
}

long HuboMotor::getUpperLimit(){
    return this->mpos2;
}

long HuboMotor::getLowerLimit(){
    return this->mpos1;
}

long HuboMotor::getKp(){
    return this->Kp;
}

long HuboMotor::getKi(){
    return this->Ki;
}

long HuboMotor::getKd(){
    return this->Kd;
}

long HuboMotor::getKpt(){
    return this->Kpt;
}

long HuboMotor::getKdt(){
    return this->Kdt;
}

long HuboMotor::getKf(){
    return this->Kf;
}

long HuboMotor::getDz(){
    return this->dz;
}

long HuboMotor::getOff(){
    return this->off;
}

long HuboMotor::getHlim(){
    return this->hlim;
}

long HuboMotor::getHld(){
    return this->hld;
}

long HuboMotor::getHv1(){
    return this->hv1;
}

long HuboMotor::getHv2(){
    return this->hv2;
}

long HuboMotor::getHma(){
    return this->hma;
}

long HuboMotor::getSm(){
    return this->sm;
}

long HuboMotor::getErs(){
    return this->ers;
}

long HuboMotor::getAs(){
    return this->as;
}

long HuboMotor::getMd(){
    return this->md;
}

long HuboMotor::getVmax(){
    return this->v_max;
}

long HuboMotor::getAmax(){
    return this->a_max;
}

long HuboMotor::getJamLim(){
    return this->jam_lim;
}

long HuboMotor::getJamd(){
    return this->jamd;
}

long HuboMotor::getPwmLim(){
    return this->pwm_lim;
}

long HuboMotor::getIerr(){
    return i_err;
}

long HuboMotor::getBerr(){
    return b_err;
}

long HuboMotor::getTicksPosition(){
    return this->ticks_position;
}

long HuboMotor::getDesiredPosition(){
	return this->desired_position;
}

bool HuboMotor::requiresMotion(){
	return desired_position - ticks_position != 0;
}

double HuboMotor::ticksToRadians(long ticks){
	return (ticks * (double)(this->drive * 2 * M_PI))/(this->driven * this->harm * this->enc);
}

long HuboMotor::radiansToTicks(double rad){
	return (long)(rad * ((double)(this->driven * this->harm * this->enc))/(this->drive * 2 * M_PI));
}

long HuboMotor::interpolate(int MAX_STEP, int MIN_STEP){
	const float LEAP_PERCENTAGE = .5;
	const int FREQUENCY = 100; //Hertz
	if (MAX_STEP == 0)
		MAX_STEP = radiansToTicks(omega) / 100;

	int error = desired_position - ticks_position;
	int output = desired_position;

	if((abs(error) > MIN_STEP)){
		output = (int)(LEAP_PERCENTAGE * error);

		if(abs(output) > MAX_STEP)
			output = output < 0 ? -MAX_STEP : MAX_STEP;

	} else
		output = error;

	output += ticks_position;
	ticks_position = output;
	return output;
}

double HuboMotor::interpolate(){
	const float LEAP_PERCENTAGE = .5;
	const double MIN_STEP = .00000001;
	const double MAX_STEP = interVel/100; //Radians per second, divided by our operating frequency.

	double error = currGoal - interStep;
	if (error == 0) return currGoal;
	std::cout << error;
	double output = currGoal;

	if((abs(error) > MIN_STEP)){
		output = (LEAP_PERCENTAGE * error);

		if(abs(output) > MAX_STEP)
			output = output < 0 ? -MAX_STEP : MAX_STEP;

	} else
		output = error;

	output += interStep;
	interStep = output;
	return interStep;
}

void HuboMotor::setName(string name){
	this->name = name;
}

void HuboMotor::setGoalPosition(double rads){
	currGoal = rads;
}

void HuboMotor::setInterVelocity(double omega){
	interVel = omega;
}

void HuboMotor::update(double position, double velocity, double temperature, double current){
	currPos = position;
	currVel = velocity;
	currTemp = temperature;
	currCurrent = current;
}

void HuboMotor::setEnabled(bool enabled){
	this->enabled = enabled;
	interStep = currPos; //If we have recently changed from non-interpolation to interpolation, the step MUST be updated.
}

void HuboMotor::setHomed(bool homed){
	this->homed = homed;
}

void HuboMotor::setZeroed(bool zeroed){
	this->zeroed = zeroed;
}

string HuboMotor::getName(){
	return name;
}

double HuboMotor::getGoalPosition(){
	return currGoal;
}

double HuboMotor::getPosition(){
	return currPos;
}

double HuboMotor::getVelocity(){
	return currVel;
}

double HuboMotor::getTemperature(){
	return currTemp;
}

double HuboMotor::getCurrent(){
	return currCurrent;
}

bool HuboMotor::isEnabled(){
	return enabled;
}

bool HuboMotor::isHomed(){
	return homed;
}

bool HuboMotor::isZeroed(){
	return zeroed;
}

<commit_msg>more debug.<commit_after>#include "HuboMotor.h"
#include <string>
#include <iostream>

HuboMotor::HuboMotor(){
    this->ticks_position = 0;
    this->desired_position = 0;
    this->omega = MAX_ANGULAR_VELOCITY;

    //NEW_DATA
    currGoal = 0;
	interStep = 0;
	interVel = 0;

	currVel = 0;
	currPos = 0;
	currCurrent = 0;
	currTemp = 0;

	enabled = false;
	homed = false;
	zeroed = false;
}

HuboMotor::HuboMotor(long mpos1, long mpos2, long kp, long kd, long ki,
                     long dz, long off, long hlim, long hld, long hv1,
                     long hv2, long hma, long sm, long ers, long as, long md,
                     long v_max, long a_max, long jam_lim, long jamd,
                     long pwm_lim, long i_err, long b_err){
    this->mpos1 = mpos1;
    this->mpos2 = mpos2;
    this->Kp = kp;
    this->Kd = kd;
    this->Ki = ki;
    this->dz = dz;
    this->off = off;
    this->hlim = hlim;
    this->hld = hld;
    this->hv1 = hv1;
    this->hv2 = hv2;
    this->hma = hma;
    this->sm = sm;
    this->ers = ers;
    this->as = as;
    this->md = md;
    this->v_max = v_max;
    this->a_max = a_max;
    this->jam_lim = jam_lim;
    this->jamd = jamd;
    this->pwm_lim = pwm_lim;
    this->i_err = i_err;
    this->b_err = b_err;
    this->ticks_position = 0;
    this->desired_position = 0;
    this->omega = MAX_ANGULAR_VELOCITY;

    currGoal = 0;
	interStep = 0;
	interVel = 0;

	currVel = 0;
	currPos = 0;
	currCurrent = 0;
	currTemp = 0;

	enabled = false;
	homed = false;
	zeroed = false;
}

HuboMotor::HuboMotor(const HuboMotor& rhs){
    this->mpos1 = rhs.mpos1;
    this->mpos2 = rhs.mpos2;
    this->Kp = rhs.Kp;
    this->Kd = rhs.Kd;
    this->Ki = rhs.Ki;
    this->dz = rhs.dz;
    this->off = rhs.off;
    this->hlim = rhs.hlim;
    this->hld = rhs.hld;
    this->hv1 = rhs.hv1;
    this->hv2 = rhs.hv2;
    this->hma = rhs.hma;
    this->sm = rhs.sm;
    this->ers = rhs.ers;
    this->as = rhs.as;
    this->md = rhs.md;
    this->v_max = rhs.v_max;
    this->a_max = rhs.a_max;
    this->jam_lim = rhs.jam_lim;
    this->jamd = rhs.jamd;
    this->pwm_lim = rhs.pwm_lim;
    this->i_err = rhs.i_err;
    this->b_err = rhs.b_err;
    this->ticks_position = rhs.ticks_position;
    this->desired_position = rhs.desired_position;
    this->omega = rhs.omega;

    //NEW_DATA
    this->name = rhs.name;
    this->currGoal = rhs.currGoal;
    this->interStep = rhs.interStep;
    this->interVel = rhs.interVel;

    this->currVel = rhs.currVel;
    this->currPos = rhs.currPos;
    this->currCurrent = rhs.currCurrent;
    this->currTemp = rhs.currTemp;

    this->enabled = rhs.enabled;
    this->homed = rhs.homed;
    this->zeroed = rhs.zeroed;
}

void HuboMotor::setUpperLimit(long limit){
    this->mpos2 = limit;
}

void HuboMotor::setLowerLimit(long limit){
    this->mpos1 = limit;
}

void HuboMotor::setPositionGain(long kp, long kd, long ki){
    this->Kp = kp;
    this->Kd = kd;
    this->Ki = ki;
}

void HuboMotor::setCurrentGain(long kpt, long kdt, long kf){
    this->Kpt = kpt;
    this->Kdt = kdt;
    this->Kf = kf;
}

void HuboMotor::setDeadZone(int dz){
    this->dz = dz;
}

void HuboMotor::setHomeset1(long off, long hlim, long hld){
    this->off = off;
    this->hlim = hlim;
    this->hld = hld;
}

void HuboMotor::setHomeset2(long hv1, long hv2, long hma, long sm){
    this->hv1 = hv1;
    this->hv2 = hv2;
    this->hma = hma;
    this->sm = sm;
}

void HuboMotor::setEncoderResolution(long ers, long as, long md){
    this->ers = ers;
    this->as = as;
    this->md = md;
}

void HuboMotor::setSpeedLimit(long vel, long acc){
    this->v_max = vel;
    this->a_max = acc;
}

void HuboMotor::setJamPowerLimit(long jam_lim, long jamd, long pwm_lim){
    this->jam_lim = jam_lim;
    this->jamd = jamd;
    this->pwm_lim = pwm_lim;
}

void HuboMotor::setErrorLimit(long i_err, long b_err){
    this->i_err = i_err;
    this->b_err = b_err;
}

void HuboMotor::setGearRatios(long drive, long driven, long harm, long enc){
	this->drive = drive;
	this->driven = driven;
	this->harm = harm;
	this->enc = enc;
}

void HuboMotor::setTicksPosition(long ticks){
    this->ticks_position = ticks;
}

void HuboMotor::setDesiredPosition(long ticks){
	this->desired_position = ticks;
}

void HuboMotor::setAngularVelocity(double omega){
	this->omega = omega;
}

long HuboMotor::getUpperLimit(){
    return this->mpos2;
}

long HuboMotor::getLowerLimit(){
    return this->mpos1;
}

long HuboMotor::getKp(){
    return this->Kp;
}

long HuboMotor::getKi(){
    return this->Ki;
}

long HuboMotor::getKd(){
    return this->Kd;
}

long HuboMotor::getKpt(){
    return this->Kpt;
}

long HuboMotor::getKdt(){
    return this->Kdt;
}

long HuboMotor::getKf(){
    return this->Kf;
}

long HuboMotor::getDz(){
    return this->dz;
}

long HuboMotor::getOff(){
    return this->off;
}

long HuboMotor::getHlim(){
    return this->hlim;
}

long HuboMotor::getHld(){
    return this->hld;
}

long HuboMotor::getHv1(){
    return this->hv1;
}

long HuboMotor::getHv2(){
    return this->hv2;
}

long HuboMotor::getHma(){
    return this->hma;
}

long HuboMotor::getSm(){
    return this->sm;
}

long HuboMotor::getErs(){
    return this->ers;
}

long HuboMotor::getAs(){
    return this->as;
}

long HuboMotor::getMd(){
    return this->md;
}

long HuboMotor::getVmax(){
    return this->v_max;
}

long HuboMotor::getAmax(){
    return this->a_max;
}

long HuboMotor::getJamLim(){
    return this->jam_lim;
}

long HuboMotor::getJamd(){
    return this->jamd;
}

long HuboMotor::getPwmLim(){
    return this->pwm_lim;
}

long HuboMotor::getIerr(){
    return i_err;
}

long HuboMotor::getBerr(){
    return b_err;
}

long HuboMotor::getTicksPosition(){
    return this->ticks_position;
}

long HuboMotor::getDesiredPosition(){
	return this->desired_position;
}

bool HuboMotor::requiresMotion(){
	return desired_position - ticks_position != 0;
}

double HuboMotor::ticksToRadians(long ticks){
	return (ticks * (double)(this->drive * 2 * M_PI))/(this->driven * this->harm * this->enc);
}

long HuboMotor::radiansToTicks(double rad){
	return (long)(rad * ((double)(this->driven * this->harm * this->enc))/(this->drive * 2 * M_PI));
}

long HuboMotor::interpolate(int MAX_STEP, int MIN_STEP){
	const float LEAP_PERCENTAGE = .5;
	const int FREQUENCY = 100; //Hertz
	if (MAX_STEP == 0)
		MAX_STEP = radiansToTicks(omega) / 100;

	int error = desired_position - ticks_position;
	int output = desired_position;

	if((abs(error) > MIN_STEP)){
		output = (int)(LEAP_PERCENTAGE * error);

		if(abs(output) > MAX_STEP)
			output = output < 0 ? -MAX_STEP : MAX_STEP;

	} else
		output = error;

	output += ticks_position;
	ticks_position = output;
	return output;
}

double HuboMotor::interpolate(){
	const float LEAP_PERCENTAGE = .5;
	const double MIN_STEP = .00000001;
	const double MAX_STEP = interVel/100; //Radians per second, divided by our operating frequency.

	double error = currGoal - interStep;
	if (error == 0) return currGoal;
	std::cout << MAX_STEP << std::endl;
	std::cout << error << std::endl;
	double output = currGoal;

	if((abs(error) > MIN_STEP)){
		output = (LEAP_PERCENTAGE * error);

		if(abs(output) > MAX_STEP)
			output = output < 0 ? -MAX_STEP : MAX_STEP;

	} else
		output = error;

	output += interStep;
	interStep = output;
	cout << output << std::endl;
	return interStep;
}

void HuboMotor::setName(string name){
	this->name = name;
}

void HuboMotor::setGoalPosition(double rads){
	currGoal = rads;
}

void HuboMotor::setInterVelocity(double omega){
	interVel = omega;
}

void HuboMotor::update(double position, double velocity, double temperature, double current){
	currPos = position;
	currVel = velocity;
	currTemp = temperature;
	currCurrent = current;
}

void HuboMotor::setEnabled(bool enabled){
	this->enabled = enabled;
	interStep = currPos; //If we have recently changed from non-interpolation to interpolation, the step MUST be updated.
}

void HuboMotor::setHomed(bool homed){
	this->homed = homed;
}

void HuboMotor::setZeroed(bool zeroed){
	this->zeroed = zeroed;
}

string HuboMotor::getName(){
	return name;
}

double HuboMotor::getGoalPosition(){
	return currGoal;
}

double HuboMotor::getPosition(){
	return currPos;
}

double HuboMotor::getVelocity(){
	return currVel;
}

double HuboMotor::getTemperature(){
	return currTemp;
}

double HuboMotor::getCurrent(){
	return currCurrent;
}

bool HuboMotor::isEnabled(){
	return enabled;
}

bool HuboMotor::isHomed(){
	return homed;
}

bool HuboMotor::isZeroed(){
	return zeroed;
}

<|endoftext|>