FLIGHT SOFTWARE 2020/07/20

include/director.h

@@ -5,7 +5,7 @@
 #include "servo.h"
 #include "sensors.h"
 
-#define SERVO_NUM (int)6
+#define SERVO_NUM (int)8
 
 #define PHASE_LOCK = 10000 // jak długo zablokować zmianę fazy na następną? minimalny czas trwania fazy lotu.
 

include/servo.h

@@ -39,4 +39,6 @@ class MyServo {
 	void setClosed(uint8_t servo);
 	bool getStatus(uint8_t servo);
 	bool ready();
+	void openSequence();
+	void closeSequence();
 };

src/director.cpp

@@ -1,8 +1,8 @@
 #include "director.h"
 #include <EEPROM.h>
 
-Flight::Flight() : servos(SERVO_NUM) {
-
+Flight::Flight() : servos(SERVO_NUM)
+{
 }
 
 void Flight::init()
@@ -142,18 +142,14 @@ void Flight::tick()
 			if (flight.sampling == false) // run once
 			{
 				Serial.println(" Sampling ON");
-
 				Serial.println("Open sequence start");
-
-				for (uint8_t i = 1; i <= SERVO_NUM; ++i)
-				{
-					servos.setOpen(i);
 
-					
-				}
+				servos.openSequence();
+
 				flight.sampling = true;
 			}
 
+
 			servos.tick();
 
 			// Check if it's time to switch state
@@ -182,22 +178,13 @@ void Flight::tick()
 				}
 			}
 
-
 			break;
 		case 3:							 // Sampling OFF
 			if (flight.sampling == true) // run once
 			{
 				Serial.println(" Sampling OFF - Safing");
 				Serial.println("Close sequence start");
-				Serial.print("Set ");
-				for (uint8_t i = 1; i <= SERVO_NUM; ++i)
-				{
-					servos.setClosed(i);
-
-					Serial.print(i);
-					Serial.print(", ");
-				}
-				Serial.println(" to Closed");
+				servos.closeSequence();
 				flight.sampling = false;
 			}
 
@@ -237,7 +224,6 @@ void Flight::tick()
 			}
 			break;
 		}
-
 
 		this->saveFlightToEEPROM();
 
@@ -250,6 +236,9 @@ void Flight::nextPhase()
 	static unsigned long status_change_tick = 0;
 	static unsigned long time_since_change = 0;
 	static unsigned long min_duration = 60000;
+
+	// TODO: Czy chcemy dodatkowy warunek?
+	//if(flight.phase == 2) min_duration = 20*60000;
 
 	time_since_change = millis() - status_change_tick;
 
@@ -280,7 +269,8 @@ void Flight::setPhase(int phase)
 	Serial.println(flight.phase);
 }
 
-void Flight::readFlightFromEEPROM() {
+void Flight::readFlightFromEEPROM()
+{
 
 	flight.phase = EEPROM.read(0);
 	flight.ground = EEPROM.read(sizeof(int));
@@ -289,7 +279,8 @@ void Flight::readFlightFromEEPROM() {
 	flight.finished = EEPROM.read(sizeof(int) + sizeof(bool) * 3);
 }
 
-void Flight::saveFlightToEEPROM() {
+void Flight::saveFlightToEEPROM()
+{
 	EEPROM.update(0, flight.phase);
 	EEPROM.update(sizeof(int), flight.ground);
 	EEPROM.update(sizeof(int) + sizeof(bool), flight.inFlight);

src/servo.cpp

@@ -1,88 +1,116 @@
 #include "servo.h"
 
-
-MyServo::MyServo(unsigned int number) {
+MyServo::MyServo(unsigned int number)
+{
 	pwm = new Adafruit_PWMServoDriver(ADDRESS);
 	servoNumber = number;
 	lastOperation = millis();
 };
 
-void MyServo::init() {
+void MyServo::init()
+{
 
 	Serial.println(" INITIALIZING SERVOS ");
 
 	pinMode(SERVO_DC, OUTPUT);
-	digitalWrite(SERVO_DC, LOW);
-
+	digitalWrite(SERVO_DC, HIGH);
+	delay(100);
 	pwm->begin();
-
 	pwm->setOscillatorFrequency(27000000);
-	pwm->setPWMFreq(SERVO_FREQ); 
+	pwm->setPWMFreq(SERVO_FREQ);
 }
 
-void MyServo::reset() {
-	Serial.print("\n RESET: ");
-	for(uint8_t i = 0; i <= servoNumber; ++i) {
+void MyServo::reset()
+{
+	for (uint8_t i = 0; i <= servoNumber; ++i)
+	{
 		pwm->setPWM(i, 0, 0);
 	}
-	digitalWrite(SERVO_DC, LOW);
-	Serial.println(" POWER OFF ");
-	delay(100);
 }
 
-void MyServo::setOpen(uint8_t servo) {
-	servos[servo-1].desired = 1;
+void MyServo::setOpen(uint8_t servo)
+{
+	servos[servo - 1].desired = 1;
 
 	Serial.print("Set ");
 	Serial.print(servo);
 	Serial.println(" to Open");
 }
 
-void MyServo::setClosed(uint8_t servo) {
-	servos[servo-1].desired = 0;
+void MyServo::setClosed(uint8_t servo)
+{
+	servos[servo - 1].desired = 0;
 	Serial.print("Set ");
 	Serial.print(servo);
 	Serial.println(" to Closed");
 }
 
-bool MyServo::getStatus(uint8_t servo) {
-	return servos[servo-1].status;
+bool MyServo::getStatus(uint8_t servo)
+{
+	return servos[servo - 1].status;
 }
 
-void MyServo::tick() {		
-	if(servo_pointer > servoNumber-1) servo_pointer = 0;
-	if(millis() - lastOperation > SERVO_SAMPLING_TIME) {
-		Serial.println("\n \n TICK");
+void MyServo::tick()
+{
+	if (servo_pointer > servoNumber - 1)
+		servo_pointer = 0;
+	if (millis() - lastOperation > SERVO_SAMPLING_TIME)
+	{
 		reset();
-		if(servos[servo_pointer].desired != servos[servo_pointer].status) {
-			Serial.print(" Moving... ");
+		if (servos[servo_pointer].desired != servos[servo_pointer].status)
+		{
 			move(servo_pointer + 1);
 			servo_pointer++;
-		} else if(!ready()) {
-			Serial.println(" Skipping to next tick ");
+		}
+		else if (!ready())
+		{
 			servo_pointer++;
 			tick();
 		}
-
-		Serial.println("\n");
 	}
 }
 
-bool MyServo::ready() {
-	for(uint8_t i = 0; i <= servoNumber; ++i) {
-		if(servos[i].desired != servos[i].status) return 0;
+bool MyServo::ready()
+{
+	for (uint8_t i = 0; i <= servoNumber; ++i)
+	{
+		if (servos[i].desired != servos[i].status)
+			return 0;
 	}
 	return 1;
 }
 
-void MyServo::move(uint8_t servo) {
+void MyServo::move(uint8_t servo)
+{
 	digitalWrite(SERVO_DC, HIGH);
 	Serial.println(" POWER ON ");
 	delay(100);
-	pwm->setPWM(servo, 0, servos[servo-1].desired ? SERVOMIN : SERVOMAX);
-	servos[servo-1].status = servos[servo-1].desired;
+	pwm->setPWM(servo, 0, servos[servo - 1].desired ? SERVOMIN : SERVOMAX);
+	delay(1500);
+	servos[servo - 1].status = servos[servo - 1].desired;
 	lastOperation = millis();
-
-	Serial.print(servos[servo-1].desired ? "Opened " : "Closed ");
+	Serial.print(servos[servo - 1].desired ? "Opened " : "Closed ");
 	Serial.println(servo);
+	digitalWrite(SERVO_DC, LOW);
+	delay(100);
+}
+
+void MyServo::openSequence()
+{
+	init();
+
+	for (uint8_t i = 1; i <= servoNumber; ++i)
+	{
+		setOpen(i);
+	}
+}
+
+void MyServo::closeSequence()
+{
+	init();
+
+	for (uint8_t i = 1; i <= servoNumber; ++i)
+	{
+		setClosed(i);
+	}
 }