101 lines
2 KiB
C++
101 lines
2 KiB
C++
#include "definitions.h"
|
|
#include "vacuum.h"
|
|
#include "servo.h"
|
|
#include "bodytemp.h"
|
|
#include "behaviour.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();
|
|
|
|
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);
|
|
|
|
|
|
|
|
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 imu_pitch=0;
|
|
float imu_roll=0;
|
|
|
|
updateMatrixWeights(loopmillis,body_present,contact_main,contact_peripheralL,contact_peripheralR,imu_pitch,imu_roll);
|
|
updateServosByWeights(loopmillis);
|
|
|
|
//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("angleX : ");
|
|
Serial.print(getAccX());
|
|
Serial.print("\tangleY : ");
|
|
Serial.print(getAccY());
|
|
Serial.print("\tangleZ : ");
|
|
Serial.print(getAccZ());
|
|
|
|
Serial.print("\t "); Serial.print(bellyup?"Belly Up":"");
|
|
|
|
|
|
|
|
Serial.println();
|
|
}
|
|
|
|
}
|