104 lines
2.5 KiB
C++
104 lines
2.5 KiB
C++
#include "definitions.h"
|
|
#include "vacuum.h"
|
|
#include "servo.h"
|
|
#include "bodytemp.h"
|
|
#include "behaviour.h"
|
|
#include "barometer.h"
|
|
#include "imu.h"
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
|
|
pinMode(PIN_VIBRATION,OUTPUT);
|
|
analogWrite(PIN_VIBRATION,0);
|
|
|
|
pinMode(PIN_BUTTON,INPUT_PULLDOWN);
|
|
pinMode(PIN_LED,OUTPUT);
|
|
digitalWrite(PIN_LED,HIGH); //LOW=Light, HIGH=Off
|
|
|
|
initVacuum();
|
|
initBodytemp();
|
|
|
|
initBarometer();
|
|
|
|
initIMU(7.54,-15.26,-1.41);
|
|
|
|
|
|
pinMode(PIN_INPUTARM1,INPUT);
|
|
pinMode(PIN_INPUTARM2,INPUT);
|
|
pinMode(PIN_INPUTARM3,INPUT);
|
|
|
|
pinMode(PIN_INPUTAUX,INPUT);
|
|
|
|
initServos();
|
|
}
|
|
|
|
|
|
|
|
|
|
void loop() {
|
|
unsigned long loopmillis=millis();
|
|
|
|
|
|
bool body_present=checkBodypresence(loopmillis);
|
|
digitalWrite(PIN_LED,!body_present);
|
|
|
|
loopVacuum(loopmillis);
|
|
|
|
|
|
loopBarometer(loopmillis);
|
|
loopIMU(loopmillis);
|
|
|
|
bool bellyup=isBellyUp();
|
|
|
|
|
|
bool buttonstate=digitalRead(PIN_BUTTON); //Temporary Vacuum Button. HIGH=Pressed
|
|
if (buttonstate) { //Button pressed
|
|
setVacuum(false); //release
|
|
}else{
|
|
setVacuum(body_present);
|
|
}
|
|
|
|
bool contact_main=digitalRead(PIN_INPUTARM1); //Sensor on tip of arm
|
|
bool contact_peripheralL=digitalRead(PIN_INPUTARM2); //Sensor on tip of arm
|
|
bool contact_peripheralR=digitalRead(PIN_INPUTARM3); //Sensor on tip of arm
|
|
|
|
float pitch=-getAngleY();
|
|
float roll=-getAngleX();
|
|
|
|
|
|
|
|
updateMood(loopmillis,getGyroAngleX(),getGyroAngleY(),getGyroAngleZ(),bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,getBarometerHeight(),getAmbienttemp(),getObjecttemp(),getAveragePumpspeed(),getAverageServoPositionerror());
|
|
updateMatrixWeights(loopmillis,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,pitch,roll);
|
|
updateServosByWeights(loopmillis,pitch,roll);
|
|
|
|
loopServos(loopmillis);
|
|
|
|
|
|
|
|
|
|
|
|
//Print Debug Information
|
|
static unsigned long last_print=0;
|
|
if (loopmillis - last_print >100) {
|
|
last_print=loopmillis;
|
|
//printVacuumValues();
|
|
//Serial.print(" Vac="); Serial.print(vac);
|
|
|
|
//printServoDebug();
|
|
//Serial.println();
|
|
//printBodytempDebug();
|
|
|
|
|
|
Serial.print("Servo Error:");Serial.print(getAverageServoPositionerror()); Serial.println();
|
|
Serial.print("Avg Pump:");Serial.print(getAveragePumpspeed()); Serial.println();
|
|
Serial.print("Height:");Serial.print(getBarometerHeight()); Serial.println();
|
|
|
|
Serial.print("GyroAngle: ");Serial.print(getGyroAngleX()); Serial.print(", ");Serial.print(getGyroAngleY()); Serial.print(", ");Serial.print(getGyroAngleZ()); Serial.println();
|
|
|
|
|
|
|
|
Serial.println();
|
|
}
|
|
|
|
}
|