//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);
}
}
}
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);
}
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;
}
