/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Main task loop
while (1) {
if(xTaskGetTickCount() < 10000) {
// For first 5 seconds use accels to get gyro bias
accelKp = 1;
// Decrease the rate of gyro learning during init
accelKi = .5 / (1 + xTaskGetTickCount() / 5000);
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
/**
* Perform the actual movement
*/
void Organism::move() {
// Do not move more than once in an update
if (hasMoved || !active)
return;
double angle, translation = 0;
// Calculate the combined angle and speed of the robots
combineVectors(angle, translation);
// Move all the agents using the combined angle
// and speed.
moveOrganism(angle, translation);
// Did we collide with anything?
if (detectCollisions()) {
// Then roll the movement back
rollbackMove();
} else {
updateWorldModels();
}
// Anyway, we are done moving this movement
this->hasMoved = true;
// So update the sensors
updateSensors();
}
void Drive::update() {
updateSensors();
// Determine elapsed time since last update()
struct timeval currenttime;
gettimeofday(¤ttime, NULL);
float dtime = currenttime.tv_sec - mLastUpdate.tv_sec
+ (currenttime.tv_usec - mLastUpdate.tv_usec) / 1000000.0f;
mLastUpdate = currenttime;
mTargetYaw += mRotate * dtime;
// Calculate average sensor readings over time
Vector3<float> accel = averageAccelerometer();
Vector3<float> gyro = averageGyroscope();
// Adjust for calibration
accel.x -= mAccelOffset.x;
accel.y -= mAccelOffset.y;
accel.z -= mAccelOffset.z;
gyro.x -= mGyroOffset.x;
gyro.y -= mGyroOffset.y;
gyro.z -= mGyroOffset.z;
calculateOrientation(dtime, accel, gyro);
stabilize(gyro);
try {
for (int i = 0; i < 4; ++i)
mMotors[i]->update();
} catch (Exception &e) {
std::cout << "Motor update failure." << std::endl;
}
}
void Everything::run()
{
updateSensors(); //call each st::Sensor object to refresh data
#ifndef DISABLE_SMARTTHINGS
SmartThing.run(); //call the ST Shield Library to receive any data from the ST Hub
updateNetworkState(); //call the ST Shield Library to update the current zigbee network status between the Shield and Hub
#endif
#if defined(ENABLE_SERIAL)
readSerial(); //read data from the Arduino IDE Serial Monitor window (useful for debugging sometimes)
#endif
sendStrings(); //send any pending updates to ST Cloud
#ifndef DISABLE_REFRESH //Added new check to allow user to disable REFRESH feature - setting is in Constants.h)
if ((bTimersPending == 0) && ((millis() - refLastMillis) >= long(Constants::DEV_REFRESH_INTERVAL) * 1000)) //DEV_REFRESH_INTERVAL is set in Constants.h
{
refLastMillis = millis();
refreshDevices(); //call each st::Device object to refresh data (this is just a safeguard to ensure the state of the Arduino and the ST Cloud stay in synch should an event be missed)
}
#endif
if(debug && millis()%60000==0 && millis()!=lastmillis)
{
lastmillis = millis();
Serial.print(F("Everything: Free Ram = "));
Serial.println(freeRam());
}
}
/**
* update the agent position in the environment. Apply simple physics (ie. obstacle collision detection and consequences).
* if collision, translation and rotation speed are set to zero.
* note: __recursiveIt is currently unused (maybe used for debug purpose, eg. checking for infinite loop.)
*/
void RobotAgent::move(int __recursiveIt) // the interface btw agent and world -- in more complex envt, this should be handled by the "world".
{
// apply world dynamics onto this agent
// compute real valued delta (convert to x/y delta coordinates)
applyDynamics();
tryMove();
updateSensors();
}
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);
}
static void updateSensorsAndScreen() {
#ifndef GPS_ENABLED
PORTD |= LED;
#else //GPS_ENABLED
if (gGpsLastData.fix != 0) {
PORTD |= LED;
}
else {
PORTD ^= LED;
}
#ifdef DEBUG
//testCalcHome();
#endif // DEBUG
if (gHomePosSet) {
calcHome(gGpsLastValidData.pos.latitude,
gGpsLastValidData.pos.longitude,
gHomePos.latitude,
gHomePos.longitude,
&gHomeDistance,
&gHomeBearing);
#ifdef STATISTICS_ENABLED
if (gHomeDistance > gStatMaxDistance) {
gStatMaxDistance = gHomeDistance;
}
#endif //STATISTICS_ENABLED
}
#endif //GPS_ENABLED
#ifdef ADC_ENABLED
measureAnalog();
#endif //ADCENABLED
#ifdef SENSORS_ENABLED
updateSensors();
#endif
#ifdef ALARM_ENABLED
updateAlarms();
#endif // ALARM_ENABLED
#ifdef GPS_ENABLED
if (gGpsLastValidData.speed < STATISTICS_MIN_SPEED_SHOW) {
if (gStatisticsShowCount < STATISTICS_DELAY_SHOW) {
gStatisticsShowCount += 1;
}
}
if (gStatisticsShowCount == STATISTICS_DELAY_SHOW) {
gStatisticsShow = 1;
}
#endif //GPS_ENABLED
}
task usercontrol()
{
nMotorEncoder[LeftBackMotor2] = 0;
//motor[Flash] = 127;
while (true) {
//displayLCDCenteredString(0, "Hello");
StartTask(Display_LCD);
updateSensors(); // updates all sensors
UC_drive(); // allows for movement of the robot
UC_arm(1); // allows for movement of the arm
UC_intake();
}
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
void System::update()
{
updateSensors();
unsigned long dt = millis() - lastRadioUpdate;
if(dt >= RADIO_UPDATE_RATE_MS) {
digitalWrite(INFO_LED_PIN, HIGH);
updatePacketData();
lastRadioUpdate = millis();
sendPacket();
digitalWrite(INFO_LED_PIN, LOW);
resetWatchdog();
#if DEBUG
Serial.print("Radio update, dt= ");
Serial.println(dt);
#endif
}
}
void PIDLineFollow::lineFollow() {
bool inside;
int sensors[N_SENSOR] = {0}, sum = 0, last_error = 0;
do {
updateSensors(sensors);
int error = (sensors[0] * EXTREM_FACTOR) + sensors[1] * CLOSE_FACTOR - sensors[3] * CLOSE_FACTOR - (sensors[4] * EXTREM_FACTOR);
error = map(error, - EXTREM_FACTOR * 100 - CLOSE_FACTOR * 100, EXTREM_FACTOR * 100 + CLOSE_FACTOR * 100, -100, 100);
int vel = error * KP / 1000 + (error - last_error) * KD / 1000 + sum * KI / 1000;
last_error = error;
sum += error;
int motor_left = constrain((ROBOT_SPEED - vel), -100, 100);
int motor_right = constrain((ROBOT_SPEED + vel), -100, 100);
RobotMotor.motorsWritePct(motor_left, motor_right);
inside = checkLine(sensors);
delay(INTEGRATION_TIME);
} while (inside);
RobotMotor.motorsStop();
}
task main(){
init(); //Initiate pieces
WaitForStart(); //Wait for the FCS to say go
StartTask(joystickOne); //Start joystick polling tasks
StartTask(joystickTwo);
while(true){ //Main execution loop
if(DEBUGMODE){DEBUG();}
if(bDisconnected){ //Stop Robot if disconnected
allStop();
continue;
}
updateSensors();
//Place any autonomous executions during teleop here
}
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent)
{
qDebug("Test");
setupUi(this);
bt = new Bluetooth("00:16:53:03:9D:7E");
if (!bt->getNxtSocket()) {
statusbar->showMessage("Fehler");
}
else {
statusbar->showMessage(QString("connected %1").arg(bt->getNxtSocket()));
}
update = new QTimer(this);
update->setInterval(1000);
connect(update, SIGNAL(timeout()), this, SLOT(updateSensors()));
update->start();
motorLeft = new Motor(bt, OUT_C);
motorRight = new Motor(bt, OUT_B);
motorLeft->setSync();
motorRight->setSync();
motorThrottle->setMaximum(100);
connect(motorThrottle, SIGNAL(valueChanged(int)), throttleDisplay,
SLOT(setNum(int)));
sonar = new Ultrasonic(bt, IN_4);
light = new Light(bt, IN_3);
connect(buttonUp, SIGNAL(pressed()), this, SLOT(goForward()));
connect(buttonUp, SIGNAL(released()), this, SLOT(stop()));
connect(buttonDown, SIGNAL(pressed()), this, SLOT(goBackward()));
connect(buttonDown, SIGNAL(released()), this, SLOT(stop()));
connect(buttonLeft, SIGNAL(pressed()), this, SLOT(goLeft()));
connect(buttonLeft, SIGNAL(released()), this, SLOT(stop()));
connect(buttonRight, SIGNAL(pressed()), this, SLOT(goRight()));
connect(buttonRight, SIGNAL(released()), this, SLOT(stop()));
}
void updateAttitude(void)
{
updateSensors();
static uint32_t now, last;
float dT;
now = micros();
dT = (float)(now - last) * 1e-6f;
last = now;
AHRSUpdate( stateData.gyro[ROLL], -stateData.gyro[PITCH], stateData.gyro[YAW],
stateData.accel[X], stateData.accel[Y], stateData.accel[Z],
stateData.mag[X], stateData.mag[Y], stateData.mag[Z],
dT);
zeroSensorAccumulators();
Quaternion2RPY(stateData.q, &stateData.roll, &stateData.pitch, &stateData.yaw);
stateData.heading += cfg.magDeclination * DEG2RAD;
}
void loop() {
if (state != 0)
{
updateSensors();
}
switch (state) {
case 0:
doState0();
break;
case 1:
doState1();
break;
case 2:
doState2();
break;
case 3:
doState3();
break;
case 4:
doState4();
break;
case 5:
doState5();
break;
}
// process serial in...
// while (Serial.available()) {
// char c = Serial.read();
// commandProcessor.update(c);
// }
}
void HarrierSim::simLoop()
{
//set the trusters
float thVar = 5.0;
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsUpThruster+randomDoubleFromNormal(thVar),0, 0,0,itsHarrierLength/4);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsUpThruster+randomDoubleFromNormal(thVar),0, 0,0,-itsHarrierLength/4);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsUpThruster+randomDoubleFromNormal(thVar),0, itsHarrierLength/4,0,0);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsUpThruster+randomDoubleFromNormal(thVar),0, -itsHarrierLength/4,0,0);
dBodyAddRelForceAtRelPos(itsHarrierBody,itsPanThruster,0,0, 0,0,itsHarrierLength/2);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsPitchThruster,0, 0,0,itsHarrierLength/2);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,-itsPitchThruster,0, 0,0,-1*itsHarrierLength/2);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsRollThruster,0, itsHarrierWidth*2,0,0);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,-itsRollThruster,0, -1*itsHarrierWidth*2,0,0);
//Apply a viscosity water force
//applyHydrodynamicForces(0.5);
//Folow the sub with the camera
// const double *bodyPos = dBodyGetPosition(itsSubBody);
//const double *bodyR = dBodyGetRotation(itsSubBody);
const dReal *bodyPos = dBodyGetPosition(itsHarrierBody);
const dReal *bodyR = dBodyGetRotation(itsHarrierBody);
updateSensors(bodyPos, bodyR);
dSpaceCollide (space,this,&nearCallback); //check for collisions
dWorldStep(world,0.1);
dJointGroupEmpty (contactgroup); //delete the contact joints
}
void SensorSystemManager::update (void)
{
updateEcmEffects();
updateSensors();
// updateTeamContactLists();
}
void raiseArm(int target) {
while(armRotation < target) {
powerArm(127);
updateSensors();
}
}
void lowerArm(int target) {
while(armRotation > target) {
powerArm(-127);
updateSensors();
}
}
void SCKAmbient::execute(boolean instant) {
if (terminal_mode) { // Telnet (#data + *OPEN* detectado )
sleep = false;
digitalWrite(AWAKE, HIGH);
server_.json_update(0, value, time, true);
usb_mode = false;
terminal_mode = false;
}
if (!RTCupdatedSinceBoot && !base_.RTCisValid(time)) {
digitalWrite(AWAKE, HIGH);
#if debugEnabled
if (!debugON) Serial.println(F("RTC not updated!!!"));
if (!debugON) Serial.println(F("With no valid time it's useless to take readings!!"));
if (!debugON) Serial.println(F("Trying to get valid time..."));
#endif
if (base_.connect()){
#if debugEnabled
if (!debugON) Serial.println(F("SCK Connected to Wi-Fi!!"));
#endif
if (server_.RTCupdate(time)) {
RTCupdatedSinceBoot = true;
if (sleep) {
base_.sleep();
digitalWrite(AWAKE, LOW);
}
#if debugEnabled
if (!debugON) Serial.println(F("RTC Updated!!"));
#endif
} else {
#if debugEnabled
if (!debugON) Serial.println(F("RTC Update Failed!!"));
#endif
}
} else {
#if debugEnabled
if (!debugON) {
Serial.print(F("Wifi conection failed!!! Using ssid: "));
printNetWorks(DEFAULT_ADDR_SSID, false);
Serial.print(F(" and pass: "******"");
Serial.println(F("Try restarting your kit!!"));
}
#endif
}
} else {
if ((millis()-timetransmit) >= (unsigned long)TimeUpdate*second || instant) {
if(!instant) timetransmit = millis(); // Only reset timer if execute() is called by timer
TimeUpdate = base_.readData(EE_ADDR_TIME_UPDATE, INTERNAL); // Time between transmissions in sec.
NumUpdates = base_.readData(EE_ADDR_NUMBER_UPDATES, INTERNAL); // Number of readings before batch update
if (!debugON) { // CMD Mode False
updateSensors(sensor_mode);
if ((sensor_mode)>NOWIFI) server_.send(sleep, &wait_moment, value, time, instant);
#if USBEnabled
txDebug();
#endif
}
}
}
}
//--------------------------------------------------------------
void ofApp::update(){
#if ARDUINO
arduino.update();
#endif
updateSensors();
}
// This update function should be called every second
void Control::update() {
updateSensors();
updatePids();
updateActuators();
mutex->update();
}
void solveMaze()
{
//Reset maze to 0
initializeMaze(&maze);
//We Know at the starting point, there's something behind us
setN(&maze[0][0], 1);
mouse.direction.x = 0;
mouse.direction.y = -1;
mouse.x = 0;
mouse.y = 0;
//Disable the mouse for now
enableDrive(0);
turnOnTimers(0, 0);
int areWeSearching = UserInterfaceIntro();
//Does the user want to skip the search phase and load a maze from EEPROM
if(areWeSearching)
{
/* SEARCH MAZE */
for(int i = 0; i < 10; i++)
{
turnOnLeds(7);
_delay_ms(10);
turnOnLeds(0);
_delay_ms(90);
}
//Init Mouse
enableDrive(1);
turnOnTimers(1, 1);
setDirection(0, 0);
//Update Sensors
updateSensors();
updateSensors();
updateWalls();
//Go Forward first block
mouse.IR_LONG_CHECK_LEFT = 2;
mouse.IR_LONG_CHECK_RIGHT = 2;
straight(480, 0, mouse.maxVelocity, mouse.maxVelocity, mouse.acceleration, mouse.deceleration);
mouse.x += mouse.direction.x;
mouse.y -= mouse.direction.y;
/* SEARCH */
InitialSearchRun();
//Search is Complete!
updateWalls();
/* TURN AROUND */
mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0;
StopFromSpeedHalf();
mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0;
moveBackwards();
//Save Maze to EEPROM
saveCurrentMaze();
writeMemByte(MOUSE_DIRECTION, firstTurn);
mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0;
moveForwardHalf();
//Current Mouse Direction
int dirx = mouse.direction.x;
int diry = mouse.direction.y;
//Reverse and go back
mouse.direction.x = -dirx;
mouse.direction.y = -diry;
//Set Position to next block
mouse.x += mouse.direction.x;
mouse.y -= mouse.direction.y;
/* RETURN SEARCH*/
ReturnSearchRun();
//Turn Around
mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0;
StopFromSpeedHalf();
mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0;
moveBackwards();
//Save Maze to EEPROM
saveCurrentMaze();
writeMemByte(MOUSE_DIRECTION, firstTurn);
/* PICK UP AND PLACE MOUSE */
enableDrive(0);
waitForButtonPressed();
}
/* FAST RUN */
FastRun();
//Completed Search run, go back and search some more
turnAroundInPlace();
floodFill(maze, firstTurn, mouse.x, mouse.y);
/* RETURN */
ReturnSearchRun();
//Turn Around
mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0;
StopFromSpeedHalf();
mouse.rightMotor.stepCount = mouse.leftMotor.stepCount = 0;
moveBackwards();
//Save Maze to EEPROM
saveCurrentMaze();
writeMemByte(MOUSE_DIRECTION, firstTurn);
stopMouse();
while(!isButtonPushed(1));
printMaze(maze);
}
