Пример #1
0
/**
 * 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();
}
Пример #3
0
void Drive::update() {

	updateSensors();

	// Determine elapsed time since last update()
	struct timeval currenttime;
	gettimeofday(&currenttime, 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;
	}
}
Пример #4
0
	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());
		}
	}
Пример #5
0
/**
 * 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();
}
Пример #6
0
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);
}
Пример #7
0
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
}  
Пример #8
0
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();
	}
}
Пример #9
0
/**
 * 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();
}
Пример #12
0
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
	}
}
Пример #13
0
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()));
}
Пример #14
0
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;
}
Пример #15
0
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);
//	}

}
Пример #16
0
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

}
Пример #17
0
void SensorSystemManager::update (void) 
{
	updateEcmEffects();
	updateSensors();
//	updateTeamContactLists();
}
Пример #18
0
void raiseArm(int target) {
	while(armRotation < target) {
		powerArm(127);
		updateSensors();
	}
}
Пример #19
0
void lowerArm(int target) {
	while(armRotation > target) {
		powerArm(-127);
		updateSensors();
	}
}
Пример #20
0
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
        }
      }
    }
  }
Пример #21
0
//--------------------------------------------------------------
void ofApp::update(){
#if ARDUINO
    arduino.update();
#endif
    updateSensors();
}
Пример #22
0
// This update function should be called every second
void Control::update() {
    updateSensors();
    updatePids();
    updateActuators();
    mutex->update();
}
Пример #23
0
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);
}