// the loop routine runs over and over again forever:
void loop() {
// TODO add print status
sampleSensors();
speedSteeringControlMap();
adjustForLeanMode();
steerControl();
leanControl();
brakeControl();
hydraulicControl();
tractionMotorCommandProcessing();
setThrottle();
setRevValues();
//analog inputs
joystickValy = readAnalog(joystickFBSensor);
joystickValx = readAnalog(joystickLRSensor);
int PumpVal = readAnalog(A2);
int BrakeSensor = readAnalog(A3);
//PWM inputs
int SteerSensor = readAnalog(A4);
int LeanSensor = readAnalog(A4);
int SpeedSensor = readAnalog(A4);
PWMOutput2(joystickValx, SteerSensor, 10);
button_test();
led_test();
digitalWrite(pSpeedSenseIn, HIGH);
delay(30); // delay in between reads for stability
}
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 Engines::disarm(){
if (!_armed) return;
setThrottle(0);
allStop();
_armed = false;
}
void Balaenidae::doControl(struct controlregister regs)
{
if (getThrottle()==0 and regs.thrust != 0)
honk();
setThrottle(-regs.thrust*2*5);
Balaenidae::rudder = -regs.roll;
}
int main(int argc, char** argv) {
initTruck();
int start_time = timeInSecs();
long double start_lat = getLatitude();
long double start_lon = getLongitude();
int start_heading = getHeading();
int phase = 0;
setSteering(0);
setThrottle(50);
while(TRUE){
int target_heading;
if(phase == 0){
if(distance(start_lat, getLatitude(), start_lon, getLongitude())>5){
setThrottle(30);
setSteering(60);
phase = 1;
target_heading = newHeading(90);
}
}
else if (phase == 1){
if(getHeading()==target_heading){
setSteering(-60);
target_heading=start_heading;
phase = 2;
}
}
else if (phase == 2){
if(getHeading()==target_heading){
setSteering(0);
setThrottle(0);
while(TRUE);
}
}
background();
}
return (EXIT_SUCCESS);
}
void Buggy::doControl(Controller controller)
{
//engine[0] = -controller.roll;
//engine[1] = controller.yaw;
//engine[2] = -controller.pitch;
//steering = -controller.precesion;
setThrottle(-controller.registers.pitch);
xRotAngle = controller.registers.precesion;
yRotAngle = controller.registers.roll;
}
void sig_source_f_base::construct()
{
Resource_impl::_started = false;
loadProperties();
serviceThread = 0;
sentEOS = false;
outputPortOrder.resize(0);
setThrottle(true);
PortableServer::ObjectId_var oid;
float_out = new bulkio::OutFloatPort("float_out");
oid = ossie::corba::RootPOA()->activate_object(float_out);
registerOutPort(float_out, float_out->_this());
outputPortOrder.push_back("float_out");
}
void Vehicle::setInput(const VehicleInput & input)
{
setSteering(input.controls[VehicleInput::STEER]);
setThrottle(input.controls[VehicleInput::THROTTLE]);
setBrake(input.controls[VehicleInput::BRAKE]);
setHandBrake(input.controls[VehicleInput::HBRAKE]);
setClutch(1 - input.controls[VehicleInput::CLUTCH]);
setNOS(input.controls[VehicleInput::NOS]);
setGear(transmission.getGear() + input.shiftgear);
autoshift = (input.logic & VehicleInput::AUTOSHIFT);
autoclutch = (input.logic & VehicleInput::AUTOCLUTCH);
setABS(input.logic & VehicleInput::ABS);
setTCS(input.logic & VehicleInput::TCS);
if (input.logic & VehicleInput::STARTENG)
startEngine();
if (input.logic & VehicleInput::RECOVER)
rolloverRecover();
}
void initTruck(){
//Setup Debugging Connection
initUART1();
//Check for any errors
checkErrorCodes();
//Setup GPS
initGPS();
initTimer4();
//Setup Input and Output
initPWM(0b11,0b11);
PWMOutputCalibration(1,HUNTER_TRUCK_STEERING_SCALE_FACTOR, HUNTER_TRUCK_STEERING_OFFSET);
PWMOutputCalibration(2,HUNTER_TRUCK_THROTTLE_SCALE_FACTOR, HUNTER_TRUCK_THROTTLE_OFFSET);
setThrottle(0);
setSteering(0);
//Setup Datalink
// initDataLink();
}
bool GameDriver::prerenderUpdate(float frameTime)
{
auto car = mWorld.getCar();
if(!mBrake)
car->setThrottle(mThrottle);
if(!mThrottle)
car->setBrake(mBrake);
if((mSteeringVelocity > 0.0f && mSteering < 0.0f) ||
(mSteeringVelocity < 0.0f && mSteering > 0.0f))
mSteering = 0.0f;
if(mSteeringVelocity) {
mSteering += mSteeringVelocity * frameTime;
mSteering = Common::clamp(-1.0f, mSteering, 1.0f);
}
if(!mSteeringWithMouse && mSteeringVelocity == 0.0f && mSteering != 0.0f) {
if(fabs(mSteering) < 4.0f * frameTime)
mSteering = 0.0f;
else if(mSteering < 0.0f)
mSteering += 4.0f * frameTime;
else if(mSteering > 0.0f)
mSteering -= 4.0f * frameTime;
}
car->setSteering(mSteering);
mWorld.updatePhysics(frameTime);
mZoom += mZoomSpeed * frameTime;
mZoom = mRenderer.setZoom(mZoom);
mRenderer.setSteering(mThrottle, mBrake, mSteering);
if(mDebugDisplay.check(frameTime)) {
mRenderer.updateDebug(&mWorld);
}
return false;
}
CX10::CX10() {
randomSeed((analogRead(A0) & 0x1F) | (analogRead(A1) << 5));
for (int n = 0; n < CRAFT; ++n) {
Craft *c = &craft_[n];
c->nextPacket = 0;
memset(c->aid, 0xff, sizeof c->aid);
memset(c->Servo_data, 0, sizeof c->Servo_data);
setThrottle(n, 0);
setAileron(n, 500);
setElevator(n, 500);
setRudder(n, 500);
for(uint8_t i=0;i<4;i++) {
#ifdef RFDUINO
c->txid[i] = random(256);
#else
c->txid[i] = random();
#endif
}
c->txid[1] %= 0x30;
c->freq[0] = (c->txid[0] & 0x0F) + 0x03;
c->freq[1] = (c->txid[0] >> 4) + 0x16;
c->freq[2] = (c->txid[1] & 0x0F) + 0x2D;
c->freq[3] = (c->txid[1] >> 4) + 0x40;
}
pinMode(ledPin, OUTPUT);
//RF module pins
pinMode(MOSI_pin, OUTPUT);
pinMode(SCK_pin, OUTPUT);
pinMode(MISO_pin, INPUT);
pinMode(CS_pin, OUTPUT);
pinMode(CE_pin, OUTPUT);
digitalWrite(ledPin, LOW);//start LED off
CS_on;//start CS high
CE_on;//start CE high
MOSI_on;//start MOSI high
SCK_on;//start sck high
delay(70);//wait 70ms
CS_off;//start CS low
CE_off;//start CE low
MOSI_off;//start MOSI low
SCK_off;//start sck low
delay(100);
CS_on;//start CS high
delay(10);
#ifdef RFDUINO
SPI.begin();
SPI.setFrequency(250);
#endif
//############ INIT1 ##############
CS_off;
_spi_write(0x3f); // Set Baseband parameters (debug registers) - BB_CAL
_spi_write(0x4c);
_spi_write(0x84);
_spi_write(0x67);
_spi_write(0x9c);
_spi_write(0x20);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x3e); // Set RF parameters (debug registers) - RF_CAL
_spi_write(0xc9);
_spi_write(0x9a);
_spi_write(0xb0);
_spi_write(0x61);
_spi_write(0xbb);
_spi_write(0xab);
_spi_write(0x9c);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x39); // Set Demodulator parameters (debug registers) - DEMOD_CAL
_spi_write(0x0b);
_spi_write(0xdf);
_spi_write(0xc4);
_spi_write(0xa7);
_spi_write(0x03);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x30); // Set TX address 0xCCCCCCCC
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x2a); // Set RX pipe 0 address 0xCCCCCCCC
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
CS_on;
delayMicroseconds(5);
_spi_write_address(0xe1, 0x00); // Clear TX buffer
_spi_write_address(0xe2, 0x00); // Clear RX buffer
_spi_write_address(0x27, 0x70); // Clear interrupts
_spi_write_address(0x21, 0x00); // No auto-acknowledge
_spi_write_address(0x22, 0x01); // Enable only data pipe 0
_spi_write_address(0x23, 0x03); // Set 5 byte rx/tx address field width
_spi_write_address(0x25, 0x02); // Set channel frequency
_spi_write_address(0x24, 0x00); // No auto-retransmit
_spi_write_address(0x31, PACKET_LENGTH); // 19-byte payload
_spi_write_address(0x26, 0x07); // 1 Mbps air data rate, 5dbm RF power
_spi_write_address(0x50, 0x73); // Activate extra feature register
_spi_write_address(0x3c, 0x00); // Disable dynamic payload length
_spi_write_address(0x3d, 0x00); // Extra features all off
MOSI_off;
delay(100);//100ms delay
//############ INIT2 ##############
healthy_ = _spi_read_address(0x10) == 0xCC;
_spi_write_address(0x20, 0x0e); // Power on, TX mode, 2 byte CRC
MOSI_off;
delay(100);
bindAllowed_ = true;
nextBind_ = micros();
nextSlot_ = 0;
}
void Thrust::reset() {
autoThrustState = OFF;
if (autoThrust.get() == true) setThrottle(0.0);
output.wrench.force.z = 0.0;
}
void Thrust::setOperatingPoint(double value) {
setThrottle(value / 255.0);
}
void Thrust::setThrust(double value) {
setThrottle(value / 255.0 / 4 / PWM2N);
}
bool Activities::ExitVehicle::update(CharacterObject *character,
CharacterController *controller) {
RW_UNUSED(controller);
if (jacked) {
const auto jacked_lhs = AnimCycle::CarJackedLHS;
const auto jacked_rhs = AnimCycle::CarJackedRHS;
const auto cycle_current = character->getCurrentCycle();
if (cycle_current == jacked_lhs || cycle_current == jacked_rhs) {
if (character->animator->isCompleted(AnimIndexAction)) {
return true;
}
} else {
if (character->getCurrentVehicle() == nullptr) return true;
auto vehicle = character->getCurrentVehicle();
auto seat = character->getCurrentSeat();
auto door = vehicle->getSeatEntryDoor(seat);
RW_UNUSED(door);
auto exitPos = vehicle->getSeatEntryPositionWorld(seat);
auto exitPosLocal = vehicle->getSeatEntryPosition(seat);
character->rotation = vehicle->getRotation();
// Exit the vehicle immediatley
character->enterVehicle(nullptr, seat);
character->setPosition(exitPos);
if (exitPosLocal.x > 0.f) {
character->playCycle(jacked_rhs);
} else {
character->playCycle(jacked_lhs);
}
// No need to open the door, it should already be open.
}
return false;
}
if (character->getCurrentVehicle() == nullptr) return true;
auto vehicle = character->getCurrentVehicle();
auto seat = character->getCurrentSeat();
auto door = vehicle->getSeatEntryDoor(seat);
auto exitPos = vehicle->getSeatEntryPositionWorld(seat);
auto exitPosLocal = vehicle->getSeatEntryPosition(seat);
auto cycle_exit = AnimCycle::CarGetOutLHS;
if (exitPosLocal.x > 0.f) {
cycle_exit = AnimCycle::CarGetOutRHS;
}
if (vehicle->getVehicle()->vehicletype_ == VehicleModelInfo::BOAT) {
auto ppos = character->getPosition();
character->enterVehicle(nullptr, seat);
character->setPosition(ppos);
return true;
}
bool isDriver = vehicle->isOccupantDriver(character->getCurrentSeat());
// If the vehicle is going too fast, slow down
if (isDriver) {
if (!vehicle->canOccupantExit()) {
vehicle->setBraking(1.f);
return false;
}
}
if (character->getCurrentCycle() == cycle_exit) {
if (character->animator->isCompleted(AnimIndexAction)) {
character->enterVehicle(nullptr, seat);
character->setPosition(exitPos);
if (isDriver) {
// Apply the handbrake
vehicle->setHandbraking(true);
vehicle->setThrottle(0.f);
}
return true;
}
} else {
character->playCycle(cycle_exit);
if (door) {
vehicle->setPartTarget(door, true, door->openAngle);
}
}
return false;
}