예제 #1
0
//Main
void main(void) {
	initBoard();
	backlightOn();

	//Configure interrupts
	//Interrupt on RB1 = SW_E
	OpenRB1INT(PORTB_CHANGE_INT_ON & FALLING_EDGE_INT & PORTB_PULLUPS_OFF & PORTB_INT_PRIO_HIGH);

	//Interrupt on RB0 = SW_W
	OpenRB0INT(PORTB_CHANGE_INT_ON & FALLING_EDGE_INT & PORTB_PULLUPS_OFF & PORTB_INT_PRIO_LOW);

	//Enable button input => not needed, see dwengoBoard.c->initBoard();
	TRISB = 0xFF;

	//Init motors
	initializeMotors();
	initializeSensors();
	
	while (TRUE) 
	{
		if(SW_N == 0)
		{
			mode = 0;
			clearLCD();
			printStringToLCD("Doing moves brah", 1, 0);
			leftMotor(700);
			rightMotor(700);
			delay_s(4);
			leftMotor(1000);
			rightMotor(700);
			delay_s(4);
			leftMotor(700);
			rightMotor(1000);
			delay_s(4);
			leftMotor(-700);
			rightMotor(700);
			delay_s(4);
		}
		else if(mode == 1)
		{
			//Light eating
			
		}
		else if(SW_S == 0)
		{
			//Start light eating
			//mode = 1;
			clearLCD();
			printStringToLCD("Going to send string", 0, 0);
			initializeRS232();
			sendData();
		}
		else
		{
			//printStringToLCD("Cool story bro", 0, 0);
		}
	}
}
예제 #2
0
void main(void) {
  /* put your own code here */
  
  unsigned int gX, gY, gZ; 
  double tcX, tcY;
  double mX, mY, mZ;
  double mSX, mSY, mSZ;
  double aX, aY, aZ;
  double faX = 0;
  double faY = 0;
  double faZ = 0;
  volatile double oldFaX = 0;
  volatile double oldFaY = 0;
  volatile double oldFaZ = 0;
  
  //ACCEL
  double pitchDeg, rollDeg, heading, tiltHeading;
  double pitchRad, rollRad;
  
  //DIST
  unsigned char strDigi[8];
  unsigned char strDist[8];
  unsigned char strVin[8];
  unsigned int i, j, k;
  unsigned char digiH;
  unsigned char digiL;
  double digi, vin, dist, oldDigi;
 
 // BMP
  double temperature;
  unsigned long pressure;
  float altSum;
  float avgAlt; 
  float old;
  float alt;
  
   

   CLKSEL = 0;                      
   SYNR = 2;
   REFDV = 0;
   while (!(CRGFLG & 0x08));
   CLKSEL = 0x80;                 // Bus = 24Mhz
   
   MCCTL = 0x4C;                  // PRE = 16 FLMC = 1
   
   EnableInterrupts;
   
   
   // INITIALIZE LCD
   DDRK = 0xFF;   
   COMWRTFIRST(0x33);   //reset sequence provided by data sheet
   COMWRT(0x32);       //reset sequence provided by data sheet
   COMWRT(0x28);       //Function set to four bit data length ,2 line, 5 x 7 dot format
   COMWRT(0x06);       //entry mode set, increment, no shift
   COMWRT(0x0E);       //Display set, disp on, cursor on, blink off
   COMWRT(0x01);       //Clear display
   COMWRT(0x80);       //set start posistion, home position
   LCDDelayDATA(TCHAR); // wait for LCD setup
   
    
  //********************************************************//
  // MOTOR Stuff
  //********************************************************//
   initializeMotors();
   for(i = 0; i < 10; i++)
   {
     thrustPOS(1, 10);
     outputDouble((PWMDTY0 << 8) + PWMDTY1);
     LCDDelaySCROLL(100);
     COMWRTFIRST(0x01);
     LCDDelayDATA(TCHAR);
   }
   
   for(j = 0; j < 10; j++)
   {
     thrustNEG(1, 10);
     outputDouble((PWMDTY0 << 8) + PWMDTY1);
     LCDDelaySCROLL(100);
     COMWRTFIRST(0x01);
     LCDDelayDATA(TCHAR);
   }
   
   /*
  
   PWMDTY0 = 0x17;    // 1ms
	 PWMDTY1 = 0x70;
	 
	 outputDouble((PWMDTY0 << 8) + PWMDTY1);
   LCDDelaySCROLL(400);
   COMWRTFIRST(0x01);
   LCDDelayDATA(TCHAR);
   
   PWMDTY0 = 0x1F;    // 1.33ms
	 PWMDTY1 = 0x40;
	 
	 outputDouble((PWMDTY0 << 8) + PWMDTY1);
   LCDDelaySCROLL(100);
   COMWRTFIRST(0x01);
   LCDDelayDATA(TCHAR);
      
   PWMDTY0 = 0x27;    // 1.65ms
	 PWMDTY1 = 0x10;
	 
	 outputDouble((PWMDTY0 << 8) + PWMDTY1);
   LCDDelaySCROLL(100);
   COMWRTFIRST(0x01);
   LCDDelayDATA(TCHAR);
   
   PWMDTY0 = 0x2E;    // 2ms
	 PWMDTY1 = 0xE0;
	 
	 outputDouble((PWMDTY0 << 8) + PWMDTY1);
   LCDDelaySCROLL(100);
   COMWRTFIRST(0x01);
   LCDDelayDATA(TCHAR);
   */
  
   while(1);
  }
예제 #3
0
void CameraController::run() {
    double averageEnergy = 0;
    double initialised = false;
    
    torque = new double[numMotors];
    previousPosition = new double[numMotors];
    angularDisplacement = new double[numMotors];
    position = new double[numMotors];
    energy = new double[numMotors];
    
    initialize();
    std::vector<Motor*> motor = initializeMotors();
    std::vector<PositionSensor*> positionSensor = initializePositionSensors();
    
    double lastTime = 0;
    while (step(TIME_STEP) != -1)
    {
        if(!initialised){
            for(int i=0;i<numMotors;i++){
                position[i] = 0;
                previousPosition[i] = 0;
            }
        }
        if(battery <= 0){
            std::cout << "We ran out of battery! " << std::endl;
            return;
        }
        double time = getTime();
        
//        camera->getImage();
//        camera->saveImage("camera-"+std::to_string(t)+".png", 100);
        for(int i=0;i<numMotors;i++){
            // Conversion calculation from
            // http://en.wikipedia.org/wiki/Torque#Units
            // E = torque * Angle
            position[i] = positionSensor[i]->getValue();                  // Radians
//            std::cout << getName() << " Current motor position: " << position[i] << " previous position: " << previousPosition[i] << std::endl;
            torque[i] = std::abs(motor[i]->getTorqueFeedback());    // Nm
            
            angularDisplacement[i] = std::abs(previousPosition[i] - position[i]);
//            std:: cout << getName() << " angular displacement: " << angularDisplacement[i] << " ( " << previousPosition[i] - position[i] << " )" << std::endl;
            
            energy[i] = torque[i] * angularDisplacement[i];
//            std:: cout << getName() << " energy cost: " << energy[i] << std::endl;
            
            if(i==0||i==2){
                motor[i]->setPosition(sin(time+i));
            }
            previousPosition[i] = position[i];
        }
        
        
        if(!isRoot()){
//            std::cout << getName() << "Sending energy usage: " << energy[0]+energy[1]+energy[2] << std::endl;
            sendEnergyUsage(energy[0]+energy[1]+energy[2]);
        }else{
            double energyUsage = energy[0]+energy[1]+energy[2];
            while(receiver->getQueueLength() > 0)
            {
                std::string message = (char*)receiver->getData();
//                std::cout << getName() << "Received message: " << message << std::endl;
                
                if (message.substr(0,15).compare("[ENERGY_UPDATE]") == 0)
                {
//                    std::cout << "Received energy usage from: " << MessagesManager::get(message,"ID") << ": " << MessagesManager::get(message,"ENERGY") << std::endl;
                    energyUsage += boost::lexical_cast<double>(MessagesManager::get(message,"ENERGY"));
                }
                
                receiver->nextPacket();
            }
            
            // energy usage in Joules
            battery -= energyUsage * EFFICIENCY_FACTOR;
            averageEnergy += energyUsage * EFFICIENCY_FACTOR;
            if(time - lastTime > 10) {
                std::cout << getName() << " Energy usage: " << averageEnergy / (time-lastTime) << " BatteryLevel: " << battery << " (" << (((double)battery/(double)batteryMax)*100.0) << "%)" << std::endl;
                lastTime = time;
                averageEnergy = 0;
            }
        }
    }

    delete [] torque;
    delete [] previousPosition;
    delete [] angularDisplacement;
    delete [] position;
    delete [] energy;
}