int main(int argc, char **argv) {
// create a simulator
simphys::SimEngine sim;
auto clock_ptr = std::make_shared< simphys::Clock<fseconds> >(fseconds{0.0f});
clock_ptr->setScale(0.01f);
sim.setClock(clock_ptr);
// create a world to simulate
auto world_ptr = std::make_shared<simphys::SimWorld>();
sim.setSimWorld(world_ptr);
// create and initialize an object
simphys::Particle p;
simphys::Sprite s;
simphys::SimObject2D testObject(p,s);
auto obj_ptr = std::make_shared<simphys::SimObject2D>(testObject);
auto objState = testObject.getState();
objState->setPosition(simphys::vec3{40, 50, 0});
objState->setVelocity(simphys::vec3{50.0, 100.0, 0});
objState->setAcceleration(simphys::vec3{0, -20.0, 0});
objState->setMass(10000.0f);
// create and initialize another object
simphys::Particle p2;
simphys::Sprite s2;
simphys::SimObject2D testObject2(p2,s2);
auto obj_ptr2 = std::make_shared<simphys::SimObject2D>(testObject2);
auto objState2 = testObject2.getState();
objState2->setPosition(simphys::vec3{550, 50, 0});
objState2->setVelocity(simphys::vec3{-50.0, 100.0, 0});
objState2->setAcceleration(simphys::vec3{0, -20.0, 0});
objState2->setMass(100.0f);
objState2->setColor(1.0f,0.0f,0.0f);
objState2->setRadius(45.0f);
// Command line velocity setting
if( argc == 4 )
{
objState->setVelocity(simphys::vec3{ float(atof(argv[1])), float(atof(argv[2])), 0});
objState2->setVelocity(simphys::vec3{ float(atof(argv[1]) * -1), float(atof(argv[2])), 0});
}
if( argc == 3 )
{
objState->setVelocity(simphys::vec3{ float(atof(argv[1])), float(atof(argv[2])), 0});
objState2->setVelocity(simphys::vec3{ float(atof(argv[1]) * -1), float(atof(argv[2])), 0});
}
// add objects to the world.
world_ptr->add(obj_ptr);
world_ptr->add(obj_ptr2);
// initialize the simulator and run it.
sim.init();
sim.run();
}
/// 2. ruch platformy niezaleznie od konfiguracji konczyn
ERR RPCCaller::movePlatformRobot(float x,float y,float z,float alpha,float beta,float gamma, float speed, int _accel)
{
setAcceleration(_accel);
if (!control->robot_rc->changePlatformRobot(x,y,z,alpha,beta,gamma,speed,4))
return 1;//poza zasiegiem stopy
return 0;
}
/// 32. krok ruchem pieciopodporowym o trajektorii trojkatnej w fazie przenoszenia
ERR RPCCaller::waveTriangleStep(float x,float y,float z,float alpha,float beta,float gamma, float speed, int _accel)
{
setAcceleration(_accel);
if (!control->shortWaveStep(1000, 1000, x, y, z, alpha, beta, gamma, 0.04, speed,accel))
return 1;//poza zasiegiem stopy
return 0;
}
CReturnTrain::CReturnTrain (CTrack* _track) : CTrain (_track) {
_track->addTrain (this);
_track->setOccupied (CTrack::OCCUPIED_RETURN);
setSpeed (-SPEED);
setAcceleration (-ACCELERATION);
install (_track->get_n() - 1); // install first coach at last point of track
}
AccelStepper::AccelStepper(void (*forward)(), void (*backward)())
{
_interface = 0;
_currentPos = 0;
_targetPos = 0;
_speed = 0.0;
_maxSpeed = 1.0;
_acceleration = 0.0;
_sqrt_twoa = 1.0;
_stepInterval = 0;
_minPulseWidth = 1;
_enablePin = 0xff;
_lastStepTime = 0;
_pin[0] = 0;
_pin[1] = 0;
_pin[2] = 0;
_pin[3] = 0;
_forward = forward;
_backward = backward;
// NEW
_n = 0;
_c0 = 0.0;
_cn = 0.0;
_cmin = 1.0;
_direction = DIRECTION_CCW;
int i;
for (i = 0; i < 4; i++)
_pinInverted[i] = 0;
// Some reasonable default
setAcceleration(1);
}
AccelStepper::AccelStepper(struct CBuffer * step_buffer)
{
_interface = STEPBUFFER;
_currentPos = 0;
_targetPos = 0;
_speed = 0.0;
_maxSpeed = 1.0;
_acceleration = 0.0;
_sqrt_twoa = 1.0;
_stepInterval = 0;
_halfstepInterval = 0;
_minPulseWidth = 1;
_enablePin = 0xff;
_lastStepTime = 0;
_pin[0] = 0;
_pin[1] = 0;
_pin[2] = 0;
_pin[3] = 0;
_step_buffer = step_buffer;
init_c_buffer(step_buffer);
// NEW
_n = 0;
_c0 = 0.0;
_cn = 0.0;
_cmin = 1.0;
_direction = DIRECTION_CCW;
int i;
for (i = 0; i < 4; i++)
_pinInverted[i] = 0;
// Some reasonable default
setAcceleration(1);
}
AccelStepper::AccelStepper(uint8_t interface, uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, bool enable)
{
_interface = interface;
_currentPos = 0;
_targetPos = 0;
_speed = 0.0;
_maxSpeed = 1.0;
_acceleration = 0.0;
_sqrt_twoa = 1.0;
_stepInterval = 0;
_minPulseWidth = 1;
_enablePin = 0xff;
_lastStepTime = 0;
_pin[0] = pin1;
_pin[1] = pin2;
_pin[2] = pin3;
_pin[3] = pin4;
// NEW
_n = 0;
_c0 = 0.0;
_cn = 0.0;
_cmin = 1.0;
_direction = DIRECTION_CCW;
int i;
for (i = 0; i < 4; i++)
_pinInverted[i] = 0;
if (enable)
enableOutputs();
// Some reasonable default
setAcceleration(1);
}
void UnstableTile::update(const sf::Time& frameTime) {
if (m_state == GameObjectState::Crumbling) {
updateTime(m_crumblingTime, frameTime);
if (m_crumblingTime == sf::Time::Zero) {
setDisposed();
}
}
else if (m_isCritical) {
updateTime(m_criticalTime, frameTime);
if (m_criticalTime == sf::Time::Zero) {
m_isFalling = true;
setState(GameObjectState::Idle);
}
}
else if (m_isFalling) {
setAcceleration(sf::Vector2f(0.f, GRAVITY_ACCELERATION));
sf::Vector2f nextPosition;
calculateNextPosition(frameTime, nextPosition);
checkCollisions(nextPosition);
}
MovableGameObject::update(frameTime);
if (m_isCritical && !m_wasCritical) {
m_criticalTime = CRITICAL_TIME;
m_isCritical = false;
setState(GameObjectState::Idle);
}
m_wasCritical = false;
}
int main(int argc, char **argv) {
// create a simulator
simphys::SimEngine sim;
auto clock_ptr = std::make_shared< simphys::Clock<fseconds> >(fseconds{0.0f});
sim.setClock(clock_ptr);
// create a world to simulate
auto world_ptr = std::make_shared<simphys::SimWorld>();
sim.setSimWorld(world_ptr);
// create and initialize an object
simphys::Particle p;
simphys::Sprite s;
simphys::SimObject2D testObject(p,s);
auto obj_ptr = std::make_shared<simphys::SimObject2D>(testObject);
auto objState = testObject.getState();
objState->setPosition(simphys::vec3{10, 20, 0});
objState->setVelocity(simphys::vec3{40.0, 60.0, 0});
objState->setAcceleration(simphys::vec3{0, -9.8, 0});
// add object to the world.
world_ptr->add(obj_ptr);
// initialize the simulator and run it.
sim.init();
sim.run();
}
void Camera::setFps( float fps )
{
this->fps = fps;
setPanSpeed();
setRotateSpeed();
setRevolveSpeed();
setAcceleration();
}
/**
* @brief
* @param 无
* @retval 无
*/
int main(void)
{
/* USART1 115200 8-N-1， */
USART1_Config();
NVIC_Configuration();
/* 10us*/
SysTick_Init();
MOTOR_GPIO_Config();
TIM2_Configuration();
TIM2_NVIC_Configuration();
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 , ENABLE);
//printf(READY);
printf("POLARGRAPH ON!");
setAcceleration(&motorA,currentAcceleration);
setAcceleration(&motorB,currentAcceleration);
mmPerStep = mmPerRev / motorStepsPerRev;
stepsPerMM = motorStepsPerRev / mmPerRev;
motorA._name = 0;
motorB._name = 1;
setAcceleration(&motorA,currentAcceleration);
setAcceleration(&motorB,currentAcceleration);
pixel_drawSquarePixel(990,486,18,23);
for(;;)
{
if(getNewComms)
{
exec_executeBasicCommand();
getNewComms = 0;
}
}
}
void MeStepperMotor::begin(byte microStep,long speed,long acceleration)
{
MeWire::begin(0x04); // join i2c bus (address optional for master)
setCurrentPosition(0);
enable();
setMicroStep(microStep);
setMaxSpeed(speed);
setAcceleration(acceleration);
}
int QDeclarativeParticleMotionGravity::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QDeclarativeParticleMotion::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
if (_id < 3)
qt_static_metacall(this, _c, _id, _a);
_id -= 3;
}
#ifndef QT_NO_PROPERTIES
else if (_c == QMetaObject::ReadProperty) {
void *_v = _a[0];
switch (_id) {
case 0:
*reinterpret_cast< qreal*>(_v) = xAttractor();
break;
case 1:
*reinterpret_cast< qreal*>(_v) = yAttractor();
break;
case 2:
*reinterpret_cast< qreal*>(_v) = acceleration();
break;
}
_id -= 3;
} else if (_c == QMetaObject::WriteProperty) {
void *_v = _a[0];
switch (_id) {
case 0:
setXAttractor(*reinterpret_cast< qreal*>(_v));
break;
case 1:
setYAttractor(*reinterpret_cast< qreal*>(_v));
break;
case 2:
setAcceleration(*reinterpret_cast< qreal*>(_v));
break;
}
_id -= 3;
} else if (_c == QMetaObject::ResetProperty) {
_id -= 3;
} else if (_c == QMetaObject::QueryPropertyDesignable) {
_id -= 3;
} else if (_c == QMetaObject::QueryPropertyScriptable) {
_id -= 3;
} else if (_c == QMetaObject::QueryPropertyStored) {
_id -= 3;
} else if (_c == QMetaObject::QueryPropertyEditable) {
_id -= 3;
} else if (_c == QMetaObject::QueryPropertyUser) {
_id -= 3;
}
#endif // QT_NO_PROPERTIES
return _id;
}
void setMD49commands(void) {
//set acceleration if command changed since last set of acceleration
if (i2cdata[2] NOT currentAcceleration) {
setAcceleration(i2cdata[2]);
}
//set new mode only if changed
if (i2cdata[3] NOT currentMode) {
setMode(i2cdata[3]);
}
//set speed continuously, when timeout is enabled
if (statusTimeout == 1) {
setSpeed1(i2cdata[0]);
setSpeed2(i2cdata[1]);
}
//set speed once changed, when timeout is disabled
if (statusTimeout == 0) {
if (recentSpeed1 != i2cdata[0]) {
setSpeed1(i2cdata[0]);
}
if (recentSpeed2 != i2cdata[1]) {
setSpeed2(i2cdata[1]);
}
}
//reset encoders if byte was checked
if (i2cdata[4] == 1) {
resetEncoders();
i2cdata[4] = 0;
}
//set regulator if changed
if (i2cdata[5] == 0) {
if (statusRegulator NOT i2cdata[5]) {
disableRegulator();
}
} else if (i2cdata[5] == 1) {
if (statusRegulator NOT i2cdata[5]) {
enableRegulator();
}
}
// set timeout if changed
if (i2cdata[6] == 0) {
if (statusTimeout NOT i2cdata[6]) {
disableTimeout();
}
} else if (i2cdata[6] == 1) {
if (statusTimeout NOT i2cdata[6]) {
enableTimeout();
}
}
}
MocapElement::MocapElement(){
ofPoint zeros = ofPoint(0.0,0.0,0.0);
for (int i = 0; i < historyDepth_; i++) {
setPosition(zeros);
setPositionFiltered(zeros);
setVelocity(zeros);
setAcceleration(zeros);
setAccelerationTrajectory(0.0);
setDistanceToTorso(0.0);
setRelativePositionToTorso(zeros);
}
}
void ObjetVolant::init()
{
//Initialise les variables
setZValue(1);
setPilotage(manuel);
setDessinerTrajectoire(false);
creerImageParDefaut();
setLooping(0);
setVitesseAngulaireObjet(180.0 / 1000.0);
setAccelerationObjet(100.0 / 1000000.0);
setVitesseMinObjet(100.0 / 1000.0);
setVitesseMaxObjet(300.0 / 1000.0);
setPosition(QPointF(0, 0));
setVitesseAngulaire(0);
setAngle(vitesseAngulaire() * 0);
setAcceleration(0);
setVitesse(vitesseMinObjet());
//Animation du looping
animationLooping = new QPropertyAnimation(this, "looping");
animationLooping->setDuration(500);
animationLoopingRoot = new QSequentialAnimationGroup(this);
animationLoopingRoot->addPause(1500); //Attend un moment avant de se retourner
animationLoopingRoot->addAnimation(animationLooping);
//Animation de la position
animationPosition = new QPropertyAnimation(this, "position");
animationPosition->setDuration(25);
//Animation de l'angle
setTransformOriginPoint(boundingRect().center()); //Met le centre de rotation au centre de l'Item
animationAngle = new QPropertyAnimation(this, "angle");
animationAngle->setDuration(25);
//Animation de la vitesse
animationVitesse = new QPropertyAnimation(this, "vitesse");
animationVitesse->setDuration(25);
//Animation principale des déplacements
animationRoot = new QParallelAnimationGroup(this);
animationRoot->addAnimation(animationAngle);
animationRoot->addAnimation(animationVitesse);
animationRoot->addAnimation(animationPosition);
connect(animationRoot, SIGNAL(finished()), this, SLOT(seDeplacer()));
temps.start();
seDeplacer();
}
ofxMocapElement::ofxMocapElement(int elementId, int depth){
elementId_ = elementId;
historyDepth_ = depth;
ofPoint zeros = ofPoint(0.0,0.0,0.0);
for (int i = 0; i < historyDepth_; i++) {
setPosition(zeros);
setPositionFiltered(zeros);
setVelocity(zeros);
setAcceleration(zeros);
setAccelerationTrajectory(0.0);
setDistanceToTorso(0.0);
setRelativePositionToTorso(zeros);
}
}
asynStatus PIGCSMotorController::setAccelerationCts( PIasynAxis* pAxis, double accelerationCts)
{
double acceleration = fabs(accelerationCts) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
if (acceleration == pAxis->m_acceleration)
return asynSuccess;
if (pAxis->m_maxAcceleration < 0)
{
getMaxAcceleration(pAxis);
}
if (acceleration > pAxis->m_maxAcceleration)
acceleration = pAxis->m_maxAcceleration;
return setAcceleration(pAxis, acceleration);
}
// (default) Velocity data function (using truth data from ownship)
bool Navigation::updateSysVelocity()
{
bool ok = false;
if (getOwnship() != 0) {
setVelocity( getOwnship()->getVelocity() );
setAcceleration( getOwnship()->getAcceleration() );
setGroundSpeedKts( getOwnship()->getGroundSpeedKts() );
setTrueAirspeedKts( getOwnship()->getTotalVelocityKts() );
setGroundTrackDeg( getOwnship()->getGroundTrackD() );
setVelocityDataValid( true );
ok = true;
} else {
setVelocityDataValid( false );
}
return ok;
}
void handleObstacles() {
for (int i = 0; i < objcount; i++) {
// Save old position
vec3d pStart = boid[i].position;
//vec3d pStartVel = boid[i].velocity;
// Compute total acceleration and then integrate
setAcceleration(boid[i], xgravity, ygravity, zgravity, windvelocity, k);
eulerIntegrate(boid[i], dt);
// Save new position for use in collision detection later
vec3d pDest = boid[i].position;
//vec3d pDestVel = boid[i].velocity;
//vec3d pDestAcc = boid[i].acceleration;
// Collision with predator's habitat
for(int pp=0; pp<habcount; pp++) {
detectAndReflect(habitat[pp], pStart, pDest, boid[i].velocity, e);
}
buildRepelWalls(num_repelvert, repelvert, boid[i].position, boid[i].velocity, raMass, raForce, REPEL);
//////////////////////////////////////////////////////
// Check if there are obstacles like a polygon along the way
// ///////////////////////////////////////////////////
for(int p=0; p<tricount; p++) {
// Check how far boids are to obstacle before reacting.
// If I don't have this, boids react too prematurely.
if(distance3d(triangle[p].topleft, boid[i].position) < 45.0) {
repelAttractVelocity(triangle[p].topleft, raMass, boid[i].position,
raForce,
REPEL,
boid[i].velocity);
repelAttractVelocity(triangle[p].topright,
raMass, boid[i].position,
raForce,
REPEL,
boid[i].velocity);
}
// Collision with other polygons
detectAndReflect(triangle[p], pStart, pDest, boid[i].velocity, e);
}
} // end for loop
}
//------------------------------------------------------------------------------
// weaponDynamics -- default dynamics; using Robot Aircraft (RAC) dynamics
//------------------------------------------------------------------------------
void Effects::weaponDynamics(const LCreal dt)
{
// Useful constant
static const LCreal g = ETHG * Basic::Distance::FT2M; // Acceleration of Gravity (m/s/s)
// ---
// Compute & Set acceleration vector (earth)
// ---
// First drag
osg::Vec3 tmp = getVelocity() * (-dragIndex);
// then gravity
osg::Vec3 ae1 = tmp;
ae1[IDOWN] += g;
setAcceleration(ae1);
// ---
// Comute & set new velocity vectory (earth)
// ---
osg::Vec3 ve1 = getVelocity() + (ae1 * dt);
setVelocity(ve1);
// ---
// .. Only after setVelocity has been called ...
// ---
LCreal vp = getTotalVelocity();
LCreal vg = getGroundSpeed();
// ---
// Set velocity vector (body)
// (total velocity is along X)
// ---
setVelocityBody(vp, 0.0, 0.0);
// ---
// Sent angular values
// ---
LCreal newPsi = lcAtan2(ve1[IEAST],ve1[INORTH]);
LCreal newTheta = lcAtan2( -ve1[IDOWN], vg );
setEulerAngles(0.0, newTheta, newPsi);
setAngularVelocities(0.0, 0.0, 0.0);
}
int QPanGesture::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QGesture::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
#ifndef QT_NO_PROPERTIES
if (_c == QMetaObject::ReadProperty) {
void *_v = _a[0];
switch (_id) {
case 0: *reinterpret_cast< QPointF*>(_v) = lastOffset(); break;
case 1: *reinterpret_cast< QPointF*>(_v) = offset(); break;
case 2: *reinterpret_cast< QPointF*>(_v) = delta(); break;
case 3: *reinterpret_cast< qreal*>(_v) = acceleration(); break;
case 4: *reinterpret_cast< qreal*>(_v) = QPanGesture::d_func()->horizontalVelocity(); break;
case 5: *reinterpret_cast< qreal*>(_v) = QPanGesture::d_func()->verticalVelocity(); break;
}
_id -= 6;
} else if (_c == QMetaObject::WriteProperty) {
void *_v = _a[0];
switch (_id) {
case 0: setLastOffset(*reinterpret_cast< QPointF*>(_v)); break;
case 1: setOffset(*reinterpret_cast< QPointF*>(_v)); break;
case 3: setAcceleration(*reinterpret_cast< qreal*>(_v)); break;
case 4: QPanGesture::d_func()->setHorizontalVelocity(*reinterpret_cast< qreal*>(_v)); break;
case 5: QPanGesture::d_func()->setVerticalVelocity(*reinterpret_cast< qreal*>(_v)); break;
}
_id -= 6;
} else if (_c == QMetaObject::ResetProperty) {
_id -= 6;
} else if (_c == QMetaObject::QueryPropertyDesignable) {
_id -= 6;
} else if (_c == QMetaObject::QueryPropertyScriptable) {
_id -= 6;
} else if (_c == QMetaObject::QueryPropertyStored) {
_id -= 6;
} else if (_c == QMetaObject::QueryPropertyEditable) {
_id -= 6;
} else if (_c == QMetaObject::QueryPropertyUser) {
_id -= 6;
}
#endif // QT_NO_PROPERTIES
return _id;
}
bool PtuWrapper::initialize()
{
// get parameters from the ros parameter server
ros::NodeHandle nh;
double spdPan, spdTilt;
double accPan, accTilt;
nh.param<std::string>("/ptu/device_file", mDeviceFile, "/dev/ttyS0");
nh.param<double>("/ptu/spdPan", spdPan, 20.0);
nh.param<double>("/ptu/spdTilt", spdTilt, 20.0);
nh.param<double>("/ptu/accPan", accPan, 100.0);
nh.param<double>("/ptu/accTilt", accTilt, 100.0);
ROS_INFO("PtuWrapper::initialize(): using device file %s, speed %f / %f deg/s and acceleration %f / %f deg/s^2 for pan/tilt.", mDeviceFile.c_str(), spdPan, spdTilt, accPan, accTilt);
mPtu = new PTU(mDeviceFile);
try
{
mPtu->init();
}
catch(PTUException e)
{
ROS_ERROR("PtuWrapper::initialize(): couldn't initialize PTU, error: %s", e.text().c_str());
return false;
}
setMaximumSpeed(spdPan, spdTilt);
usleep(100000);
setAcceleration(accPan, accTilt);
usleep(100000);
// This probably doesn't need to be configurable from the outside.
mPtu->setHoldPowerMode(PTU_PAN, HPM_LOW); // HPM_OFF, HPM_LOW, HPM_REGULAR
usleep(100000);
mPtu->setHoldPowerMode(PTU_TILT, HPM_LOW); // HPM_OFF, HPM_LOW, HPM_REGULAR
usleep(100000);
mPtu->setMovePowerMode(PTU_PAN, MPM_HIGH); // MPM_LOW, MPM_REGULAR, MPM_HIGH
usleep(100000);
mPtu->setMovePowerMode(PTU_TILT, MPM_HIGH); // MPM_LOW, MPM_REGULAR, MPM_HIGH
return true;
}
Miner::Miner() {
m_istamina = max_stamina;
m_eLocation = home;
setPosition({ 20.0f, 50.0f });
//setVelocity({ 0.5f, 0.5f }); // non dovrebbe avere una base di velocità, dovrebbe essere 0
setAcceleration({ 0.5f, 0.5f });
setMaxVelocity(2.0f);
m_eAgentType = GNOME;
m_pMinerStateMachine = new MinerStateMachine(this);
State<Miner>* m_pStartState = (State<Miner>*)&Idle::getIInstance();
m_pMinerStateMachine->SetCurrentState(m_pStartState);
m_pSteeringBehaviors = new SteeringBehaviors(this);
}
void ShiftableTile::update(const sf::Time& frameTime) {
if (m_state == GameObjectState::Crumbling) {
updateTime(m_crumblingTime, frameTime);
if (m_crumblingTime == sf::Time::Zero) {
setDisposed();
}
MovableGameObject::update(frameTime);
return;
}
setAcceleration(sf::Vector2f(m_pushAcceleration, GRAVITY_ACCELERATION));
sf::Vector2f nextPosition;
calculateNextPosition(frameTime, nextPosition);
checkCollisions(nextPosition);
MovableGameObject::update(frameTime);
m_pushAcceleration = 0.f;
if (m_boundingBox.top + m_boundingBox.height > (m_level->getWorldRect().top + m_level->getWorldRect().height)) {
setDisposed();
}
}
int main(int argc, char **argv)
{
printf("**** MD22 test application ****\n");
if ((fd = open(fileName, O_RDWR)) < 0) { // Open port for reading and writing
printf("Failed to open i2c port\n");
exit(1);
}
if (ioctl(fd, I2C_SLAVE, address) < 0) { // Set the port options and set the address of the device we wish to speak to
printf("Unable to get bus access to talk to slave\n");
exit(1);
}
buf[0] = 7; // This is the register we wish to read software version from
if ((write(fd, buf, 1)) != 1) { // Send regiter to read from
printf("Error writing to i2c slave\n");
exit(1);
}
if (read(fd, buf, 1) != 1) { // Read back data into buf[]
printf("Unable to read from slave\n");
exit(1);
}
else {
printf("Software v: %u \n",buf[0]);
}
setAcceleration(); // Set acceleration to max this will give us the longest time to reach a set speed
setMotorSpeeds(255); // speed to full forward
sleep(5); // Pause to allow MD22 to reach speed
setMotorSpeeds(0); // Full reverse
sleep(10);
setMotorSpeeds(128); // Stop
sleep(5);
return 0;
}
MotorOdom05::MotorOdom05() :
RobotDeviceCL("MotorOdom05", CLASS_MOTOR_ODOM), device_(NULL),
motorPosLeft_(0), motorPosRight_(0),
odomPosLeft_(0), odomPosRight_(0),
errorNbr_(0), reqNbr_(0)
{
device_ = IoManager->getIoDevice(IO_ID_MOTOR_ODOM_05);
if (device_ != NULL) {
if (device_->open()) {
LOG_OK("Initialization Done\n");
idle();
setSpeed(0,0);
setAcceleration(0x40);
} else {
device_=NULL;
LOG_ERROR("Device-open for MotorOdom05 failed.\n");
}
} else {
LOG_ERROR("MotorOdom05 device not found!\n");
}
}
void LevelMainCharacter::handleMovementInput()
{
float newAccelerationX = 0;
if (g_inputController->isKeyActive(Key::Left))
{
m_nextIsFacingRight = false;
newAccelerationX -= getConfiguredWalkAcceleration();
}
if (g_inputController->isKeyActive(Key::Right))
{
m_nextIsFacingRight = true;
newAccelerationX += getConfiguredWalkAcceleration();
}
if (g_inputController->isKeyJustPressed(Key::Jump) && m_isGrounded)
{
setVelocityY(m_isFlippedGravity ? getConfiguredMaxVelocityY() : -getConfiguredMaxVelocityY()); // first jump vel will always be max y vel.
}
setAcceleration(sf::Vector2f(newAccelerationX, (m_isFlippedGravity ? -getConfiguredGravityAcceleration() : getConfiguredGravityAcceleration())));
}
void InitStepper(Stepper_t* motor, uint8_t interface, uint16_t pin1, GPIO_TypeDef* GPIOxPin1, uint16_t pin2, GPIO_TypeDef* GPIOxPin2, uint16_t pin3, GPIO_TypeDef* GPIOxPin3, uint16_t pin4, GPIO_TypeDef* GPIOxPin4, uint8_t enable)
{
motor->_interface = interface;
motor->_currentPos = 0;
motor->_targetPos = 0;
motor->_speed = 0.0;
motor->_maxSpeed = 1.0;
motor->_acceleration = 0.0;
motor->_sqrt_twoa = 1.0;
motor->_stepInterval = 0;
motor->_minPulseWidth = 1;
motor->_enablePin = 0xff;
motor->_GPIOxEnablePin = NULL;
motor->_lastStepTime = 0;
motor->_pin[0] = pin1;
motor->_GPIOxPin[0] = GPIOxPin1;
motor->_pin[1] = pin2;
motor->_GPIOxPin[1] = GPIOxPin2;
motor->_pin[2] = pin3;
motor->_GPIOxPin[2] = GPIOxPin3;
motor->_pin[3] = pin4;
motor->_GPIOxPin[3] = GPIOxPin4;
// NEW
motor->_n = 0;
motor->_c0 = 0.0;
motor->_cn = 0.0;
motor->_cmin = 1.0;
motor->_direction = DIRECTION_CCW;
int i;
for (i = 0; i < 4; i++)
motor->_pinInverted[i] = 0;
if (enable)
enableOutputs(motor);
// Some reasonable default
setAcceleration(motor, 1);
}
void InitStepperFunct(Stepper_t* motor, void (*forward)(), void (*backward)())
{
motor->_interface = 0;
motor->_currentPos = 0;
motor->_targetPos = 0;
motor->_speed = 0.0;
motor->_maxSpeed = 1.0;
motor->_acceleration = 0.0;
motor->_sqrt_twoa = 1.0;
motor->_stepInterval = 0;
motor->_minPulseWidth = 1;
motor->_enablePin = 0xff;
motor->_GPIOxEnablePin = NULL;
motor->_lastStepTime = 0;
motor->_pin[0] = 0;
motor->_GPIOxPin[0] = NULL;
motor->_pin[1] = 0;
motor->_GPIOxPin[1] = NULL;
motor->_pin[2] = 0;
motor->_GPIOxPin[2] = NULL;
motor->_pin[3] = 0;
motor->_GPIOxPin[3] = NULL;
motor->_forward = forward;
motor->_backward = backward;
// NEW
motor->_n = 0;
motor->_c0 = 0.0;
motor->_cn = 0.0;
motor->_cmin = 1.0;
motor->_direction = DIRECTION_CCW;
int i;
for (i = 0; i < 4; i++)
motor->_pinInverted[i] = 0;
// Some reasonable default
setAcceleration(motor, 1);
}
