void Ship::Blastoff()
{
if (m_flightState != LANDED) return;
ClearThrusterState();
m_flightState = FLYING;
m_testLanded = false;
m_dockedWith = 0;
m_launchLockTimeout = 2.0; // two second of applying thrusters
vector3d up = GetPosition().Normalized();
Enable();
assert(GetFrame()->m_astroBody->IsType(Object::PLANET));
const double planetRadius = 2.0 + static_cast<Planet*>(GetFrame()->m_astroBody)->GetTerrainHeight(up);
SetVelocity(vector3d(0, 0, 0));
SetAngVelocity(vector3d(0, 0, 0));
SetForce(vector3d(0, 0, 0));
SetTorque(vector3d(0, 0, 0));
Aabb aabb;
GetAabb(aabb);
// XXX hm. we need to be able to get sbre aabb
SetPosition(up*planetRadius - aabb.min.y*up);
SetThrusterState(1, 1.0); // thrust upwards
Pi::luaOnShipTakeOff->Queue(this, GetFrame()->m_astroBody);
}
RigidBody::RigidBody(Rect rect)
{
float width = (rect.right - rect.left);
float height = (rect.bottom - rect.top);
// Create a Rectangle shape - origin middle
mShape = new Shape();
mShape->setOrigin(Vector(rect.left + width/2, rect.top + height/2, 0));
mShape->addPoint(Vector(-width/2, -height/2, 0)); // top - left
mShape->addPoint(Vector(-width/2, height/2, 0)); // bottom - left
mShape->addPoint(Vector(width/2, height/2, 0)); // bottom - right
mShape->addPoint(Vector(width/2, -height/2, 0)); // top - right
setStatic(false);
mAlive = true;
mTexture = NULL;
SetMass(1);
SetVelocity(0, 0);
SetAngularVelocity(0.0f);
SetForce(Vector(0, 0, 0));
SetTorque(Vector(0, 0, 0));
SetMomentum(Vector(0, 0, 0));
SetFriction(1.0f);
SetSleeping(false);
}
void RigidBody::ApplyForce(Vector force, Vector pos)
{
SetForce(GetForce() + force);
// Torque = r x F
//SetTorque(force.cross(pos));
SetTorque((pos - GetPosition()).cross(force));
}
//ToggleTorque-------------------------------
//Set holding torque on/off
//Input: mode = 0 (off) or 1 (on)
//Ouput: 1 if successful, 0 if not
int zStepperDriver::ToggleTorque()
{
int retVal;
if (bTorqueOn)
{
retVal = SetTorque(1);
//zStepDriver.bTorqueOn = false;
}
else
{
retVal = SetTorque(0);
//zStepDriver.bTorqueOn = true;
}
return 1;
}
void CloseGripper(float perc)
{
SetTorqueControl(7,1);
int bits1 = SetTorque( 7, perc);
dxl_write_word( 7, SERVO_PROG_SPEED , bits1 );
_delay_ms(30);
SetTorqueControl(8,1);
int bits2 = SetTorque( 8, perc);
dxl_write_word( 17, SERVO_PROG_SPEED , bits2 );
//printf("bits7: %d ",bits1);
//printf("bits17: %d",bits2);
desiredForce = perc;
doGripper = 1;
}
void pmsm_update()
{
static float t=0;
float a,b,c;
float p;
t+=0.001;
p = powf(t,2.)*10.;
//TODO: set rotor position
SetTorque(.5);
//SetPosition(p);
//p = 3.141592;
if(t<1.){
SetPosition(p);
} else {
p = (t+1)*150.;
SetPosition(p);
}
SetTorque(.3);
SetPosition(motor_state.rotor_state+2*3.14/6);
motor_info.newData = 0;
PMSM_Update();
pmsm_set_duty_cycle((uint16_t)(motor_info.t1/100.*4096),(uint16_t)(motor_info.t2/100.*4096),(uint16_t)(motor_info.t3/100.*4096));
//pmsm_set_duty_cycle(0,2000,2000);
/*t++;
b = sin((0.15*t)+3.14*2/3)/2+0.5;
a = sin((0.15*t))/2+0.5;
c = sin((0.15*t)+3.14*4/3)/2+0.5;
if(a>b)
if(b>c)
c=0;
else
b=0;
else
if(c>b)
a=0;
else if (c>a)
a=0;
else
c=0;
pmsm_set_duty_cycle((uint16_t)(a*4096),(uint16_t)(b*4096),(uint16_t)(c*4096));*/
}
void Ship::TestLanded()
{
m_testLanded = false;
if (m_launchLockTimeout > 0.0f) return;
if (m_wheelState < 1.0f) return;
if (GetFrame()->GetBodyFor()->IsType(Object::PLANET)) {
double speed = GetVelocity().Length();
vector3d up = GetPosition().Normalized();
const double planetRadius = static_cast<Planet*>(GetFrame()->GetBodyFor())->GetTerrainHeight(up);
if (speed < MAX_LANDING_SPEED) {
// orient the damn thing right
// Q: i'm totally lost. why is the inverse of the body rot matrix being used?
// A: NFI. it just works this way
matrix4x4d rot;
GetRotMatrix(rot);
matrix4x4d invRot = rot.InverseOf();
// check player is sortof sensibly oriented for landing
const double dot = vector3d(invRot[1], invRot[5], invRot[9]).Normalized().Dot(up);
if (dot > 0.99) {
Aabb aabb;
GetAabb(aabb);
// position at zero altitude
SetPosition(up * (planetRadius - aabb.min.y));
vector3d forward = rot * vector3d(0,0,1);
vector3d other = up.Cross(forward).Normalized();
forward = other.Cross(up);
rot = matrix4x4d::MakeRotMatrix(other, up, forward);
rot = rot.InverseOf();
SetRotMatrix(rot);
SetVelocity(vector3d(0, 0, 0));
SetAngVelocity(vector3d(0, 0, 0));
SetForce(vector3d(0, 0, 0));
SetTorque(vector3d(0, 0, 0));
// we don't use DynamicBody::Disable because that also disables the geom, and that must still get collisions
DisableBodyOnly();
ClearThrusterState();
m_flightState = LANDED;
Sound::PlaySfx("Rough_Landing", 1.0f, 1.0f, 0);
Pi::luaOnShipLanded->Queue(this, GetFrame()->GetBodyFor());
}
}
}
}
//Constructor
//Initializes hardware and software variables. Redundant for ZStepperInit
zStepperDriver::zStepperDriver()
{
int dummy=0;
// double prevPos;
port = 1;
prevPort = 1;
fullStepConvFactor = Num_Microns_Per_Full_Step; // microns/step
halfStepConvFactor = Num_Microns_Per_Half_Step;
stepMode = Default_Step_Mode;
bTorqueOn = 1;
//Try to init motor.
if (!initialized)
dummy = OpenPort();
if (dummy)
{
//MessageBox();
return;
}
dummy = SetTorque(1);
if (dummy)
bTorqueOn = true;
SetStepMode(stepMode);
switch (stepMode)
{
case 0:
currentStepConvFactor = Num_Microns_Per_Half_Step;
break;
case 1:
currentStepConvFactor = Num_Microns_Per_Full_Step;
break;
default:;
}
ReadPos(currentStepConvFactor);
}
RigidBody::RigidBody(float x, float y, int width, int height)
{
// Create a Rectangle shape - origin middle
mShape = new Shape();
mShape->setOrigin(Vector(x, y, 0));
mShape->addPoint(Vector(-width/2, -height/2, 0)); // top - left
mShape->addPoint(Vector(-width/2, height/2, 0)); // bottom - left
mShape->addPoint(Vector(width/2, height/2, 0)); // bottom - right
mShape->addPoint(Vector(width/2, -height/2, 0)); // top - right
SetMass(1);
SetVelocity(0, 0);
SetAngularVelocity(0.0f);
SetForce(Vector(0, 0, 0));
SetTorque(Vector(0, 0, 0));
SetFriction(1.0f);
SetFriction(0.5f),
SetSimulate(true);
SetOwner(NULL);
}
RigidBody::RigidBody(float x, float y, int width, int height)
{
// Create a Rectangle shape - origin middle
mShape = new Shape();
mShape->setOrigin(Vector(x, y, 0));
mShape->addPoint(Vector(-width/2, -height/2, 0)); // top - left
mShape->addPoint(Vector(-width/2, height/2, 0)); // bottom - left
mShape->addPoint(Vector(width/2, height/2, 0)); // bottom - right
mShape->addPoint(Vector(width/2, -height/2, 0)); // top - right
setStatic(false);
mAlive = true;
SetMass(1);
SetVelocity(0, 0);
SetAngularVelocity(0.0f);
SetForce(Vector(0, 0, 0));
SetTorque(Vector(0, 0, 0));
SetMomentum(Vector(0, 0, 0));
SetFriction(1.0f);
SetSleeping(false);
SetRepeatX(false);
}
