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); }