/*
*/
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "settings.h"
#include "settings_internal.h"
#include "settings_eeprom.h"
#include "channels.h"
#include "receiver.h"
#include "receiver_spi.h"
#include "buttons.h"
#include "state.h"
#include "ui.h"
#include <Servo.h>
#include <Stepper.h>
#include "timer.h" // use this one in stead
#define SIGMOID // Best
#define SIGMOID_P_SLOPE 1
#define SIGMOID_P_OFFSET 4
#define SIGMOID_T_SLOPE 1
#define SIGMOID_T_OFFSET 4
#define UP_RSSI_PIN A5 // analog RSSI measurement pin
#define SX_RSSI_PIN A6 // analog RSSI measurement pin
#define DX_RSSI_PIN A7
// down antenna value is composed by mean of SX and DX values
#define TILT_SERVO_PIN 13 // Pan servo pin
#define BACKLIGHT_PIN 25 //diplay backlight pin on off
#define I2C_SLA (0x3C) //monitor 12864. invece il 16x4 è al 0x3F
uint8_t I2C_SLA = 0x3c*2;
LiquidCrystal_I2C lcd(0x27,16,4); // set the LCD address to 0x27 or 0x20
/////motor stepper definition///////////////////////////////////////////////////////////
// change this to the number of steps on your motor
#define STEPS 4096
// create an instance of the stepper class, specifying
// the number of steps of the motor and the pins it's
// attached to
Stepper stepper(STEPS, 6, 7, 8, 9); //IN1 sul pin 6
//IN2 sul pin 7
//IN3 sul pin 8
//IN4 sul pin 9
// the previous reading from the analog input
int previous = 0;
//int count = 0;
//int count2 = 0;
//int delayTime = 500;
//int val = 0;
//int motor_Speed = 4; //Si tratta di un motore passo passo unipolare con 4096 passi per una rotazione di 360 gradi.
//int motor_Step;
//int motor_direction_cw = 0;
//int cmotor_direction_cw = 0;
#define P_STEP_MAX 360 // equals to 4096 step
#define P_STEP_MIN 0
#define P_STEP_MAX_STEP 5 // equals to 57 step
#define P_STEP_MIN_STEP 0.1 // prevents windUP
#define P_DEADBAND 1
#define P_STEP_DIRECTION 1
float anglePan = 45;
boolean debug = false;
///////////////////////////////////////////////////////////////////////////////////////////////////////////
#define RSSI_OFFSET_C 0
#define RSSI_OFFSET_B 0
#define RSSI_OFFSET_A 0
#define RSSI_OFFSET_DW 0
#define RSSI_MAX 1024
#define RSSI_MIN 120
////////////////servo tilt definition//////////////////////
#define TSERVO_MAX 90
#define TSERVO_MIN 0
#define TSERVO_MAX_STEP 5
#define TSERVO_MIN_STEP 0.1 // prevents windUP and servo crawl
#define DEADBAND 5
#define TSERVO_DIRECTION 1
#define FIR_SIZE 10
#define LAST FIR_SIZE - 1
float angleTilt = 45;
Servo servoTilt;
/////////////////////////////////////////////////////////////////
// timers
Timer t_measureRSSI = Timer(50);
Timer t_mainLoop = Timer(5);
////////////////////////////////////////////////////////77
uint16_t rssi_B_array[FIR_SIZE];
uint16_t rssi_C_array[FIR_SIZE];
uint16_t rssi_A_array[FIR_SIZE];
uint16_t rssi_DW_array[FIR_SIZE];
static void globalMenuButtonHandler(
Button button,
Buttons::PressType pressType
);
void setup()
{
// Switch on the backlight
pinMode ( BACKLIGHT_PIN, OUTPUT );
digitalWrite ( BACKLIGHT_PIN, HIGH );
lcd.begin(16,4); // initialize the lcd
lcd.setCursor(0,0);
lcd.print("RSSI UP");
lcd.setCursor(1,0);
lcd.print("RSSI DW");
lcd.setCursor(2,0);
lcd.print("RSSI SX");
lcd.setCursor(3,0);
lcd.print("RSSI DX");
/////////////////////////////////////////////////
// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
// 0O O O O O O O O O O O O O O O O O
// 1O O O O O O O O O O O O O O O O O
// 2O O O O O O O O O O O O O O O O O
// 3O O O O O O O O O O O O O O O O O
setupPins();
servoTilt.attach(TILT_SERVO_PIN);
servoTilt.write(45);
for (int i = 0; i < FIR_SIZE; i++) {
rssi_C_array[i] = 0;
rssi_B_array[i] = 0;
}
for (int i = 0; i < FIR_SIZE; i++) {
rssi_A_array[i] = 0;
rssi_DW_array[i] = 0;
}
Serial.begin(115200);
while (!debug) {
delay(7000);
debug = true;
}
////////////////////////////////////////////////////////////////////////////////////////
// set the speed of the motor in RPM
// 90 deg in 4 seconds means 3.75rpm
// 90 deg in 3 seconds means 5rpm
// 90 deg in 2 seconds means 7,5rpm
stepper.setSpeed(5);
////////////////////////////////////////////////////////////////////////////////////////
// Enable buzzer and LED for duration of setup process.
digitalWrite(PIN_LED, HIGH);
digitalWrite(PIN_BUZZER, LOW);
setupSettings();
StateMachine::setup();
Receiver::setup();
Ui::setup();
Receiver::setActiveReceiver(Receiver::ReceiverId::UP);
// #ifdef USE_IR_EMITTER
// Serial.begin(9600);
// #endif
// #ifdef USE_SERIAL_OUT
// Serial.begin(250000);
// #endif
// setup complete.
digitalWrite(PIN_LED, LOW);
digitalWrite(PIN_BUZZER, HIGH);
Buttons::registerChangeFunc(globalMenuButtonHandler);
// Switch to initial state.
StateMachine::switchState(StateMachine::State::SEARCH);
}
void setupPins() {
// pinMode(PIN_LED, OUTPUT);
// pinMode(PIN_BUZZER, OUTPUT);
pinMode(PIN_BUTTON_UP, INPUT_PULLUP);
pinMode(PIN_BUTTON_MODE, INPUT_PULLUP);
pinMode(PIN_BUTTON_DOWN, INPUT_PULLUP);
pinMode(PIN_BUTTON_SAVE, INPUT_PULLUP);
pinMode(PIN_LED_A, OUTPUT);
#ifdef USE_DIVERSITY
pinMode(PIN_LED_B, OUTPUT);
#endif
pinMode(PIN_RSSI_A, INPUT_PULLUP);
#ifdef USE_DIVERSITY
pinMode(PIN_RSSI_B, INPUT_PULLUP);
#endif
pinMode(PIN_SPI_SLAVE_SELECT, OUTPUT);
pinMode(PIN_SPI_DATA, OUTPUT);
pinMode(PIN_SPI_CLOCK, OUTPUT);
digitalWrite(PIN_SPI_SLAVE_SELECT, HIGH);
digitalWrite(PIN_SPI_CLOCK, LOW);
digitalWrite(PIN_SPI_DATA, LOW);
}
void setupSettings() {
EepromSettings.load();
Receiver::setChannel(EepromSettings.startChannel);
}
void loop() {
// update timers
if (t_measureRSSI.hasTicked()) {
measureRSSI(); // do stuff
t_measureRSSI.reset(); // restart timer
}
if (t_mainLoop.hasTicked()) {
mainLoopLR(); // execute mainLoop
t_mainLoop.reset(); // restart timer
}
if (t_mainLoop.hasTicked()) {
mainLoopUD(); // execute mainLoop
t_mainLoop.reset(); // restart timer
}
Receiver::update();
Buttons::update();
StateMachine::update();
Ui::update();
EepromSettings.update();
if (
StateMachine::currentState != StateMachine::State::SCREENSAVER
&& StateMachine::currentState != StateMachine::State::BANDSCAN
&& (millis() - Buttons::lastChangeTime) >
(SCREENSAVER_TIMEOUT * 1000)
) {
StateMachine::switchState(StateMachine::State::SCREENSAVER);
}
}
static void globalMenuButtonHandler(
Button button,
Buttons::PressType pressType
) {
if (
StateMachine::currentState != StateMachine::State::MENU &&
button == Button::MODE &&
pressType == Buttons::PressType::HOLDING
) {
StateMachine::switchState(StateMachine::State::MENU);
}
}
Arduino:1.8.12 (Windows 10), Board:"Arduino Mega or Mega 2560, ATmega2560 (Mega 2560)"
rx5808-pro-diversity-PANTILT_3HELI:40:21: error: expected unqualified-id before numeric constant
#define I2C_SLA (0x3C) //monitor 12864. invece il 16x4 è al 0x3F
^
C:\Users\nikok\Downloads\rx5808-pro-diversity-PANTILT_3HELI\rx5808-pro-diversity-PANTILT_3HELI\rx5808-pro-diversity-PANTILT_3HELI.ino:42:9: note: in expansion of macro 'I2C_SLA'
uint8_t I2C_SLA = 0x3c*2;
^~~~~~~
rx5808-pro-diversity-PANTILT_3HELI:40:21: error: expected ')' before numeric constant
#define I2C_SLA (0x3C) //monitor 12864. invece il 16x4 è al 0x3F
^
C:\Users\nikok\Downloads\rx5808-pro-diversity-PANTILT_3HELI\rx5808-pro-diversity-PANTILT_3HELI\rx5808-pro-diversity-PANTILT_3HELI.ino:42:9: note: in expansion of macro 'I2C_SLA'
uint8_t I2C_SLA = 0x3c*2;
^~~~~~~
exit status 1
expected unqualified-id before numeric constant
Dit rapport zou meer informatie bevatten met
"Uitgebreide uitvoer weergeven tijden compilatie"
optie aan in Bestand -> Voorkeuren.