void PhysicsModule::update(sf::Time deltaT)
{
float t = 1;
//C
float c = t*t*.5;
setAccel(getAccel() * c);
//B
setVel(getVel() * t);
//B&C
addVel(getAccel());
//A&B&C
addPos(getVel());
setAccel(sf::Vector2f(0,0));
}
// Drive during race.
void Driver::drive(tSituation *s)
{
memset(&car->ctrl, 0, sizeof(tCarCtrl));
update(s);
//pit->setPitstop(true);
if (isStuck()) {
car->_steerCmd = -mycardata->getCarAngle() / car->_steerLock;
car->_gearCmd = -1; // Reverse gear.
car->_accelCmd = 1.0f; // 100% accelerator pedal.
car->_brakeCmd = 0.0f; // No brakes.
car->_clutchCmd = 0.0f; // Full clutch (gearbox connected with engine).
} else {
car->_steerCmd = filterSColl(getSteer());
car->_gearCmd = getGear();
car->_brakeCmd = filterABS(filterBrakeSpeed(filterBColl(filterBPit(getBrake()))));
if (car->_brakeCmd == 0.0f) {
car->_accelCmd = filterTCL(filterTrk(filterOverlap(getAccel())));
} else {
car->_accelCmd = 0.0f;
}
car->_clutchCmd = getClutch();
}
}
int EmergenceViewer::getAccelMidiControlValue(float multiplier) {
float ret = getAccel() * multiplier;
ret = ofClamp(ret, getMinAccel(), getMaxAccel());
ret = ofMap(ret, getMinAccel(), getMaxAccel(), 1, 127);
//printf("\nva=%f, m=%f, mina=%f, maxa=%f, retf=%f, reti=%i\n", getAccel(), multiplier, getMinAccel(), getMaxAccel(), ret, (int) ret);
return (int) ret;
}
void moveEntities(float dt, const Map* map, const std::vector<Entity*>& entities, CollisionData& coldata) {
// Do not affect "non-physics" entities
std::vector<Entity*> physicsEntities;
for (size_t i=0; i<entities.size(); ++i) {
if (entities[i]->getBehavior()->getSlot<PhysicsBehaviorSlot>()) {
physicsEntities.push_back(entities[i]);
}
}
Vec2f newVel;
for (size_t i=0; i < physicsEntities.size(); ++i) {
PhysicsBehaviorSlot* phys = physicsEntities[i]->getBehavior()->getSlot<PhysicsBehaviorSlot>();
physicsEntities[i]->setNextWithCurrent();
newVel = physicsEntities[i]->getVel() + getAccel(physicsEntities[i]) * dt;
checkVelocity(newVel, phys);
physicsEntities[i]->setNextVel(newVel);
phys->inAir = true;
//the<VisDebug>().addSegment(physicsEntities[i]->getPos(),physicsEntities[i]->getPos() + physicsEntities[i]->getVel() * dt, Vec3f(1,0,1));
}
// Find collisions for all entities, and find out where they need to move (by setting next pos)
resolveCollisionsAndMove(dt,map,physicsEntities);
// Officially move the entities by making their next positions the current ones, this is needed to
// ensure that all entities collide against the same setup.
for (size_t i=0; i < physicsEntities.size(); ++i) {
physicsEntities[i]->setCurrentWithNext();
//the<VisDebug>().addSegment(physicsEntities[i]->getPos(),physicsEntities[i]->getPos() + physicsEntities[i]->getVel() * dt, Vec3f(0,0,0.5));
}
}
bool PlayerIntention::operator == (const PlayerIntention &other) const {
return getAccel() == other.getAccel()
&& getTurn() == other.getTurn()
&& getJump() == other.getJump()
&& getReset() == other.getReset()
&& getPaint() == other.getPaint()
&& getErase() == other.getErase();
}
/**
* Set the x, y and z values
*/
void getValues(){
getAccel(ACCEL, x, y, z);
// Normalize the values of the Accel sensor
if (x > 405) x -= 1024;
if (y > 405) y -= 1024;
if (z > 405) z -= 1024;
}
CarControl Curve::drive(FSMDriver5 *FSMDriver5, CarState &cs) {
float steer = getSteer(cs);
int gear = StraightLine::getGear(cs);
float accel = getAccel(cs);
float brake = getBrake(cs);
float clutch = 0;
return CarControl(accel, brake, gear, steer, clutch);
}
bool
VehicleType::handleVariable(const std::string& objID, const int variable, VariableWrapper* wrapper) {
switch (variable) {
case TRACI_ID_LIST:
return wrapper->wrapStringList(objID, variable, getIDList());
case ID_COUNT:
return wrapper->wrapInt(objID, variable, getIDCount());
case VAR_LENGTH:
return wrapper->wrapDouble(objID, variable, getLength(objID));
case VAR_HEIGHT:
return wrapper->wrapDouble(objID, variable, getHeight(objID));
case VAR_MINGAP:
return wrapper->wrapDouble(objID, variable, getMinGap(objID));
case VAR_MAXSPEED:
return wrapper->wrapDouble(objID, variable, getMaxSpeed(objID));
case VAR_ACCEL:
return wrapper->wrapDouble(objID, variable, getAccel(objID));
case VAR_DECEL:
return wrapper->wrapDouble(objID, variable, getDecel(objID));
case VAR_EMERGENCY_DECEL:
return wrapper->wrapDouble(objID, variable, getEmergencyDecel(objID));
case VAR_APPARENT_DECEL:
return wrapper->wrapDouble(objID, variable, getApparentDecel(objID));
case VAR_ACTIONSTEPLENGTH:
return wrapper->wrapDouble(objID, variable, getActionStepLength(objID));
case VAR_IMPERFECTION:
return wrapper->wrapDouble(objID, variable, getImperfection(objID));
case VAR_TAU:
return wrapper->wrapDouble(objID, variable, getTau(objID));
case VAR_SPEED_FACTOR:
return wrapper->wrapDouble(objID, variable, getSpeedFactor(objID));
case VAR_SPEED_DEVIATION:
return wrapper->wrapDouble(objID, variable, getSpeedDeviation(objID));
case VAR_VEHICLECLASS:
return wrapper->wrapString(objID, variable, getVehicleClass(objID));
case VAR_EMISSIONCLASS:
return wrapper->wrapString(objID, variable, getEmissionClass(objID));
case VAR_SHAPECLASS:
return wrapper->wrapString(objID, variable, getShapeClass(objID));
case VAR_WIDTH:
return wrapper->wrapDouble(objID, variable, getWidth(objID));
case VAR_COLOR:
return wrapper->wrapColor(objID, variable, getColor(objID));
case VAR_MINGAP_LAT:
return wrapper->wrapDouble(objID, variable, getMinGapLat(objID));
case VAR_MAXSPEED_LAT:
return wrapper->wrapDouble(objID, variable, getMaxSpeedLat(objID));
case VAR_LATALIGNMENT:
return wrapper->wrapString(objID, variable, getLateralAlignment(objID));
case VAR_PERSON_CAPACITY:
return wrapper->wrapInt(objID, variable, getPersonCapacity(objID));
default:
return false;
}
}
CarControl ApproachingCurve::drive(FSMDriver5 *FSMDriver5, CarState &cs) {
if(!sensorsAreUpdated) /*@todo Só atualiza na 1a vez mesmo? */
updateSensors(cs);
const int focus = 0, meta = 0;
const float clutch = 0;
return CarControl(getAccel(cs), getBrake(cs), getGear(cs), cs.getAngle(), clutch, focus, meta);
//Use the line below if the behavior of adjusting the car to the curve ahead is desired (not fully functional):
//return CarControl(getAccel(cs), getBrake(cs), getGear(cs), getSteering(cs), clutch, focus, meta);
}
/* Drive during race. */
void Driver::drive(tSituation *s)
{
memset(&car->ctrl, 0, sizeof(tCarCtrl));
update(s);
if (isStuck()) {
car->ctrl.steer = -angle / car->_steerLock;
car->ctrl.gear = -1; // reverse gear
car->ctrl.accelCmd = 0.5; // 50% accelerator pedal
car->ctrl.brakeCmd = 0.0; // no brakes
} else {
car->ctrl.steer = filterSColl(getSteer());
car->ctrl.gear = getGear();
car->ctrl.brakeCmd = filterABS(filterBrakeSpeed(filterBColl(filterBPit(getBrake()))));
if (car->ctrl.brakeCmd == 0.0) {
car->ctrl.accelCmd = filterTCL(filterTrk(getAccel()));
} else {
car->ctrl.accelCmd = 0.0;
}
}
}
void CiraExponentialDrag::test()
{
cout<<"testing CiraExponentialDrag"<<endl;
Vector<double> r(3),v(3);
r(0)=-4453783.586;
r(1)=-5038203.756;
r(2)=-426384.456;
v(0) = 3831.888;
v(1) = -2887.221;
v(2) = -6.018232;
EarthBody body;
UTCTime t;
Spacecraft sc;
(void)computeDensity(t,body,r,v);
doCompute(t,body,sc);
(void)getAccel();
}
CarControl
SimpleDriver::wDrive(CarState cs)
{
// check if car is currently stuck
if ( fabs(cs.getAngle()) > stuckAngle )
{
// update stuck counter
stuck++;
}
else
{
// if not stuck reset stuck counter
stuck = 0;
}
// after car is stuck for a while apply recovering policy
if (stuck > stuckTime)
{
/* set gear and sterring command assuming car is
* pointing in a direction out of track */
// to bring car parallel to track axis
float steer = - cs.getAngle() / steerLock;
int gear=-1; // gear R
// if car is pointing in the correct direction revert gear and steer
if (cs.getAngle()*cs.getTrackPos()>0)
{
gear = 1;
steer = -steer;
}
// Calculate clutching
clutching(cs,clutch);
// build a CarControl variable and return it
CarControl cc (1.0,0.0,gear,steer,clutch);
return cc;
}
else // car is not stuck
{
// compute accel/brake command
float accel_and_brake = getAccel(cs);
// compute gear
int gear = getGear(cs);
// compute steering
float steer = getSteer(cs);
// normalize steering
if (steer < -1)
steer = -1;
if (steer > 1)
steer = 1;
// set accel and brake from the joint accel/brake command
float accel,brake;
if (accel_and_brake>0)
{
accel = accel_and_brake;
brake = 0;
}
else
{
accel = 0;
// apply ABS to brake
brake = filterABS(cs,-accel_and_brake);
}
// Calculate clutching
clutching(cs,clutch);
cout << "Steer: "<< steer << endl;
cout << "Accel: :"<< accel << endl;
// build a CarControl variable and return it
CarControl cc(accel,brake,gear,steer,clutch);
return cc;
}
}
void record_shot()
{
int16_t ax=0,ay=0,az=0;
// int16_t gx,gy,gz;
// int16_t mx,my,mz;
add_l=0;
add_m=0;//reset addresses to zero
add_h=0;
data_count=0;
uart_puts("Shot Started...\n");
//get 20 seconds of data
//each sensor is 14 bytes of data
//each byte is roughtly 2.5 ms
//uint16_t i=0;
//int16_t x=0;
uint8_t data[50];
char addL[10];
//char addM[5];
//char addH[5];
count_t=0;
num_records=0;
while(1)
{
_delay_ms(500);
PORTB&=~(1<<sensor1_cs);//select sensor
SPIgetAccel(&ax,&ay,&az);
PORTB|=(1<<sensor1_cs);//deselect sensor
//getAccel(&ax,&ay,&az,MPU9250_DEFAULT_ADDRESS);
itoa(ax,addL,10);
uart_puts("X axis: ");
uart_puts(addL);
itoa(ay,addL,10);
uart_puts("Y axis: ");
uart_puts(addL);
itoa(az,addL,10);
uart_puts("Z axis: ");
uart_puts(addL);
}
//*********************************stop*******************
while(count_t<20000)//for full shot change back to 20000************
{
num_records++;
//_delay_us(1000000);
// sram_write(add_l,add_m,add_h,(uint8_t)(count_t>>8));
add_inc();
data_count++;
// sram_write(add_l,add_m,add_h,(uint8_t)(count_t));
add_inc();
data_count++;
// uart_puts("\nPre-getAccel");
uint8_t i;
for(i=0;i<3;i++)
{
_delay_us(1250);
getAccel(&ax,&ay,&az, MPU9250_DEFAULT_ADDRESS);//fetch all axis compass readings
data[0]=(int8_t)(ax>>8);
data[1]=(int8_t)ax;
data[2]=(int8_t)(ay>>8);
data[3]=(int8_t)ay;
data[4]=(int8_t)(az>>8);
data[5]=(int8_t)az;
/*
uart_puts("\nX- ");
itoa(ax,addL,10);
uart_puts(addL);
uart_puts("\tY- ");
itoa(ay,addL,10);
uart_puts(addL);
uart_puts("\tZ- ");
itoa(az,addL,10);
uart_puts(addL);
*/
/*
getGyro(&ax,&ay,&az, MPU9250_ALT_DEFAULT_ADDRESS);//fetch all axis compass readings
data[6]=(int8_t)(ax>>8);
data[7]=(int8_t)ax;
data[8]=(int8_t)(ay>>8);
data[9]=(int8_t)ay;
data[10]=(int8_t)(az>>8);
data[11]=(int8_t)az;
getAccel(&ax,&ay,&az, MPU9250_DEFAULT_ADDRESS);//fetch all axis compass readings
data[12]=(int8_t)(ax>>8);
data[13]=(int8_t)ax;
data[14]=(int8_t)(ay>>8);
data[15]=(int8_t)ay;
data[16]=(int8_t)(az>>8);
data[17]=(int8_t)az;
*/
/*
uart_puts("\nX2- ");
itoa(ax,addL,10);
uart_puts(addL);
uart_puts(" Y2- ");
itoa(ay,addL,10);
uart_puts(addL);
uart_puts(" Z2- ");
itoa(az,addL,10);
uart_puts(addL);
*/
/* getGyro(&ax,&ay,&az, MPU9250_DEFAULT_ADDRESS);//fetch all axis compass readings
data[18]=(int8_t)(ax>>8);
data[19]=(int8_t)ax;
data[20]=(int8_t)(ay>>8);
data[21]=(int8_t)ay;
data[22]=(int8_t)(az>>8);
data[23]=(int8_t)az;
*/
uint8_t _i;
for(_i=0;_i<18;_i++)
{
sram_write(add_l,add_m,add_h,data[_i]);
add_inc();
data_count++;
}
}//end for loop
}//end timing while loop
}//end record_shot
float Accelerometer::getPitch()
{
Vec3f acc = getAccel();
return (atan2(acc.x, sqrt(acc.y*acc.y + acc.z*acc.z))*180.0)/M_PI;
}
float Accelerometer::getRoll()
{
Vec3f acc = getAccel();
return (atan2(-acc.y, acc.z)*180.0)/M_PI;
}
void MouseConfig::saveParams( void )
{
if (GUI)
{
accelRate = getAccel();
thresholdMove = getThreshold();
handed = getHandedness();
}
XChangePointerControl( kapp->getDisplay(),
TRUE, TRUE, accelRate, 1, thresholdMove);
unsigned char map[5];
int remap=1;
if (handedEnabled)
{
switch (num_buttons)
{
case 1:
{
map[0] = (unsigned char) 1;
}
break;
case 2:
if (handed == RIGHT_HANDED)
{
map[0] = (unsigned char) 1;
map[1] = (unsigned char) 3;
}
else
{
map[0] = (unsigned char) 3;
map[1] = (unsigned char) 1;
}
break;
case 3:
if (handed == RIGHT_HANDED)
{
map[0] = (unsigned char) 1;
map[1] = (unsigned char) middle_button;
map[2] = (unsigned char) 3;
}
else
{
map[0] = (unsigned char) 3;
map[1] = (unsigned char) middle_button;
map[2] = (unsigned char) 1;
}
break;
case 5:
// Intellimouse case, where buttons 1-3 are left, middle, and
// right, and 4-5 are up/down
if (handed == RIGHT_HANDED)
{
map[0] = (unsigned char) 1;
map[1] = (unsigned char) 2;
map[2] = (unsigned char) 3;
map[3] = (unsigned char) 4;
map[4] = (unsigned char) 5;
}
else
{
map[0] = (unsigned char) 3;
map[1] = (unsigned char) 2;
map[2] = (unsigned char) 1;
map[3] = (unsigned char) 4;
map[4] = (unsigned char) 5;
}
break;
default:
{
//catch-all for mice with four or more than five buttons
//Without this, XSetPointerMapping is called with a undefined value
//for map
remap=0; //don't remap
}
break;
}
int retval;
if (remap)
while ((retval=XSetPointerMapping(kapp->getDisplay(), map,
num_buttons)) == MappingBusy)
/* keep trying until the pointer is free */
{ };
}
config->setGroup("Mouse");
config->writeEntry("Acceleration",accelRate);
config->writeEntry("Threshold",thresholdMove);
if (handed == RIGHT_HANDED)
config->writeEntry("MouseButtonMapping","RightHanded");
else
config->writeEntry("MouseButtonMapping","LeftHanded");
}
