void Balaenidae::doDynamics(dBodyID body)
{
Vec3f Ft;
Ft[0]=0;Ft[1]=0;Ft[2]=getThrottle();
dBodyAddRelForce(body,Ft[0],Ft[1],Ft[2]);
dBodyAddRelTorque(body,0.0f,Balaenidae::rudder*1000,0.0f);
if (offshoring == 1) {
offshoring=0;
setStatus(Balaenidae::SAILING);
}
else if (offshoring > 0)
{
// Add a retractive force to keep it out of the island.
Vec3f ap = Balaenidae::ap;
setThrottle(0.0);
Vec3f V = ap*(-10000);
dBodyAddRelForce(body,V[0],V[1],V[2]);
offshoring--;
}
// Buyoncy
//if (pos[1]<0.0f)
// dBodyAddRelForce(me,0.0,9.81*20050.0f,0.0);
dReal *v = (dReal *)dBodyGetLinearVel(body);
dVector3 O;
dBodyGetRelPointPos( body, 0,0,0, O);
dVector3 F;
dBodyGetRelPointPos( body, 0,0,1, F);
F[0] = (F[0]-O[0]);
F[1] = (F[1]-O[1]);
F[2] = (F[2]-O[2]);
Vec3f vec3fF;
vec3fF[0] = F[0];vec3fF[1] = F[1]; vec3fF[2] = F[2];
Vec3f vec3fV;
vec3fV[0]= v[0];vec3fV[1] = v[1]; vec3fV[2] = v[2];
speed = vec3fV.magnitude();
VERIFY(speed, me);
vec3fV = vec3fV * 0.02f;
dBodyAddRelForce(body,vec3fV[0],vec3fV[1],vec3fV[2]);
wrapDynamics(body);
}
void Balaenidae::doControl(struct controlregister regs)
{
if (getThrottle()==0 and regs.thrust != 0)
honk();
setThrottle(-regs.thrust*2*5);
Balaenidae::rudder = -regs.roll;
}
bool SpektrumRX::initOkay(void){
// check if all sticks are centered and land selected all is okay
if (getThrottle() == 0){
if (getRoll() == 0){
if (getPitch() == 0){
if (getYaw() == 0){
if (getAuto() == 0)
return true;
else
return false;
}
else
return false;
}
else
return false;
}
else
return false;
}
else
return false;
}
//all functions necessary for teleopperiodic
void DriveTrain::Drive(){
float throttle = getThrottle(kThrottleMinimum); //minimum value for throttle in (min, 1)
//if x is pressed let operator pivot
if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kBbutton)){
throttle=.5;
}
if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kXbutton)){
myRobot->Drive(0.5 * xBox->GetRawAxis(ControllerConstants::xBoxAxisMap::kLSXAxis),kTurnRightFullDegrees);
}//otherwise drive with each stick controlling the robot
else if (leftStick->GetRawButton(2)){
driveDistance(50, -.75); //
leftEncoder->Reset();
rightEncoder->Reset();
}
else if (rightStick->GetRawButton(2)){
driveDistance(50, .75);
leftEncoder->Reset();
rightEncoder->Reset();
}
//else if (leftStick->GetRawButton(2)){
// driveDistance(50,.75);
//}
//interesting. why is the left commented out but not the right?
else{
myRobot->TankDrive(leftStick->GetY()*throttle*-1, rightStick->GetY()*throttle*-1, true);
step=1;
}
//button RT controls power of shooter
if (xBox->GetRawAxis(ControllerConstants::xBoxAxisMap::kRTAxis)>kTriggerThreshold){
shooter->shoot(xBox->GetRawAxis(ControllerConstants::xBoxAxisMap::kRTAxis));
}
//if right bumper is pressed, shoot boulder at 100% power
else if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kRBbutton)){
shooter->shoot(kshoot100);
}
//button LB takes in the ball and resets Servo
else if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kLBbutton)){
shooter->takeInBall();
servo->SetAngle(kServoRest);
}
//button Y stops motors and resets Servo
else if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kYbutton)){
shooter->stopMotors();
servo->SetAngle(kServoRest);
}
//button RB flicks servo to send ball through motors
else if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kAbutton)){
servo->SetAngle(kServoShoot);
}
//camera->cameraTeleop();
//lift boulder at left analog stick speed
shooter->angleBall(xBox->GetRawAxis(ControllerConstants::xBoxAxisMap::kLSYAxis));
SmartDashboard::PutNumber("LD", leftEncoder->GetDistance()); //-432
SmartDashboard::PutNumber("RD", rightEncoder->GetDistance()); //630
SmartDashboard::PutNumber("angleTele", ahrs->GetAngle());
//OneStick: myRobot->ArcadeDrive(stick->GetY()*throttle, stick->GetTwist(),true);
}
