From 9b5246ba428bc3eb6e986ad10f37ee732d5c9774 Mon Sep 17 00:00:00 2001 From: yasiupl Date: Tue, 21 Jul 2020 18:35:00 +0200 Subject: [PATCH] FLIGHT SOFTWARE 2020/07/20 --- include/director.h | 2 +- include/servo.h | 2 + src/director.cpp | 35 ++++++---------- src/servo.cpp | 102 +++++++++++++++++++++++++++++---------------- 4 files changed, 81 insertions(+), 60 deletions(-) diff --git a/include/director.h b/include/director.h index 60ab903..a1204aa 100644 --- a/include/director.h +++ b/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. diff --git a/include/servo.h b/include/servo.h index 9b377b6..8913e4e 100644 --- a/include/servo.h +++ b/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(); }; \ No newline at end of file diff --git a/src/director.cpp b/src/director.cpp index 5d3c4bf..2df5300 100644 --- a/src/director.cpp +++ b/src/director.cpp @@ -1,8 +1,8 @@ #include "director.h" #include -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); diff --git a/src/servo.cpp b/src/servo.cpp index f70dbbe..915d79d 100644 --- a/src/servo.cpp +++ b/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); + } } \ No newline at end of file