Commit a42e2a62 authored by markus's avatar markus

Merge branch 'refactor' into 'master'

Refactor rot0r control

See merge request !1
parents c87c1289 fc4a6e57
#include "communicator.h"
Communicator::Communicator()
{
}
#ifndef COMMUNICATOR_H
#define COMMUNICATOR_H
#include <Arduino.h>
class Communicator
{
public:
enum messageError { MSG_OK,
MSG_NONE_AVAILABLE };
Communicator();
virtual String getCommandString(messageError*);
virtual messageError sendMessage(String);
private:
};
#endif // COMMUNICATOR_H
#include "control.h"
#include <string.h>
Control::Control(Rotor *rotor)
{
rotor_ = rotor;
serial_communicator_ = new SerialCommunicator();
udp_communicator_ = new UDPCommunicator();
debugTime_ = 0;
}
void Control::doCommunicate()
{
String command_string;
Communicator::messageError msg_r;
if((command_string = udp_communicator_->getCommandString(&msg_r)) && (msg_r == Communicator::MSG_OK)) {
executeCommandString(&command_string, udp_communicator_);
}
if((command_string = serial_communicator_->getCommandString(&msg_r)) && (msg_r == Communicator::MSG_OK)) {
executeCommandString(&command_string, serial_communicator_);
}
}
uint16 Control::setElevation(uint16 el)
{
return rotor_->setElevation(el);
}
uint16 Control::setAzimuth(uint16 az)
{
return rotor_->setAzimuth(az);
}
void Control::doControl()
{
rotor_->doRotor();
doCommunicate();
if(debugTime_ > 100)
{
//hamlib.debugOut();
//rotor_->debugOut();
debugTime_ = 0;
}
debugTime_++;
}
Control::hamlibError Control::executeCommandString(String *command_string, Communicator *communicator)
{
String substring;
int lastPos = -1;
int newPos = 0;
int dst = 0;
while (newPos = command_string->indexOf(";", lastPos + 1)) {
substring = command_string->substring(lastPos + 1, newPos);
lastPos = newPos;
if ( substring == "state" )
{
// GNAAAARRR
communicator->sendMessage(String(String(AZIM_MIN) + "/" + String(AZIM_MAX) + " " + String(ELEV_MIN) + "/" + String(ELEV_MAX)));
}
else if ( substring == "getpos" ) {
// GNAAAARRR
communicator->sendMessage(String(rotor_->getAzimuth()) + ";" + String(rotor_->getElevation()) + ";");
}
else if ( substring.startsWith("setaz") ) {
if( substring.length() < 7 )
return HAMLIB_NOK;
dst = (int)substring.substring(5).toFloat();
if ( dst == 0 && substring.substring(5) != "0" )
return HAMLIB_NOK;
if ( dst < AZIM_MIN or dst > AZIM_MAX )
return HAMLIB_NOK;
rotor_->setAzimuth(dst);
}
else if ( substring.startsWith("setel") ) {
if( substring.length() < 7 )
return HAMLIB_NOK;
dst = (int)substring.substring(5).toFloat();
if ( dst == 0 && substring.substring(5) != "0" )
return HAMLIB_NOK;
if ( dst < ELEV_MIN or dst > ELEV_MAX )
return HAMLIB_NOK;
rotor_->setElevation(dst);
}
else
return HAMLIB_NOK;
}
return HAMLIB_OK;
}
#ifndef CONTROL_H
#define CONTROL_H
#include "Arduino.h"
#include "rotor.h"
#include "serial_communicator.h"
#include "udp_communicator.h"
class Control
{
public:
enum hamlibError{HAMLIB_OK,
HAMLIB_NOK };
Control(Rotor *);
void doControl();
uint16 setAzimuth(uint16);
uint16 setElevation(uint16);
//uint16 getAz() const;
//uint16 getEl() const;
private:
void doCommunicate();
hamlibError executeCommandString(String*, Communicator*);
Rotor *rotor_;
Communicator *serial_communicator_;
Communicator *udp_communicator_;
int debugTime_;
};
#endif // CONTROL_H
......@@ -76,7 +76,7 @@ CHamlib::hamlibError CHamlib::parseHamlibPacket()
sendHamlibPacket();
}
else if ( token == "get position" ) {
sprintf(tx_packet_, "%f %f", rotor_->getActualAzimuth(), rotor_->getActualElevation());
sprintf(tx_packet_, "%f %f", rotor_->getAzimuth(), rotor_->getElevation());
packet_parsed_ = true;
sendHamlibPacket();
}
......
#include "motor.h"
Motor::Motor(double max, double min, int down_pin, int up_pin, int break_pin, int sensor_pin, double degree, double resolution, double span)
{
max_ = max;
min_ = min;
span_ = span;
degree_ = degree;
up_pin_ = up_pin;
down_pin_ = down_pin;
break_pin_ = break_pin;
sensor_pin_ = sensor_pin;
resolution_ = resolution;
//Pin Settings
pinMode(up_pin_, OUTPUT);
pinMode(down_pin_, OUTPUT);
pinMode(break_pin_, OUTPUT);
//All Relais Off
digitalWrite(up_pin_, HIGH);
digitalWrite(down_pin_, HIGH);
digitalWrite(break_pin_, HIGH);
should_rotate_ = false;
}
void Motor::doMotor()
{
double diff;
readMotSensor();
if(should_rotate_)
{
diff = position_ - destination_;
if(abs(diff) < resolution_)
stop();
else {
if(diff > 0)
rotateDown();
else
rotateUp();
}
}
}
Motor::motorError Motor::rotateUp()
{
releaseBreak();
digitalWrite(down_pin_, HIGH);
digitalWrite(up_pin_, LOW);
return Motor::ROT_OK;
}
Motor::motorError Motor::rotateDown()
{
releaseBreak();
digitalWrite(up_pin_, HIGH);
digitalWrite(down_pin_, LOW);
return Motor::ROT_OK;
}
void Motor::releaseBreak()
{
digitalWrite(break_pin_, LOW);
}
void Motor::tightenBreak()
{
digitalWrite(break_pin_, HIGH);
}
void Motor::stop()
{
should_rotate_ = false;
tightenBreak();
digitalWrite(down_pin_, HIGH);
digitalWrite(up_pin_, HIGH);
}
double Motor::getPosition() const
{
return position_;
}
Motor::motorError Motor::setPosition(double destination)
{
if(destination >= min_ && destination <= max_) {
destination_ = destination;
if(abs(destination_ - position_) > span_) {
should_rotate_ = true;
return Motor::ROT_OK;
}
return Motor::ROT_ALREADY_IN_SPAN;
}
return Motor::ROT_VAL_OUT_OF_RANGE;
}
Motor::motorError Motor::readMotSensor()
{
position_ = readADC(sensor_pin_)/degree_;
return Motor::ROT_OK;
}
double Motor::readADC(int AdcPin)
{
//Read ADC 10 times and calculate average
uint32_t adcValue = 0;
for(int i = 0; i < 10; i++)
adcValue += analogRead(AdcPin);
delay(1);
return (((double)adcValue)/10);
}
#ifndef MOTOR_H
#define MOTOR_H
#include "Arduino.h"
class Motor
{
public:
enum motorError { ROT_OK,
ROT_NOK,
ROT_VAL_OUT_OF_RANGE,
ROT_BREAK_NOT_RELEASED,
ROT_ALREADY_IN_SPAN };
Motor(double max, double min, int down_pin, int up_pin, int break_pin, int sensor_pin, double degree, double resolution, double span);
void doMotor();
double getPosition() const;
void releaseBreak();
void tightenBreak();
void stop();
motorError setPosition(double);
motorError rotateUp();
motorError rotateDown();
private:
motorError readMotSensor();
double readADC(int);
double max_;
double min_;
int down_pin_;
int up_pin_;
int break_pin_;
int sensor_pin_;
double position_;
double destination_;
double degree_;
double resolution_;
double span_;
byte should_rotate_;
};
#endif //MOTOR_H
///
/// @file rotor.cpp
/// @copyright Christian Obersteiner, Andreas Müller - CC-BY-SA 4.0
///
/// @brief Contains all kind of rotor functions
///
#include "rotor.h"
///
/// @brief Constructor
///
Rotor::Rotor()
{
initRotor();
azimuth_ = new Motor(AZIM_MAX, AZIM_MIN, AZIM_RE_CW, AZIM_RE_CCW, AZIM_RE_BREAK, AZIM_POT, AZIM_DEG, AZIM_RES, AZIM_SPAN);
elevation_ = new Motor(ELEV_MAX, ELEV_MIN, ELEV_RE_DWN, ELEV_RE_UP, ELEV_RE_BREAK, ELEV_POT, ELEV_DEG, ELEV_RES, ELEV_SPAN);
}
///
/// @brief Initialize all ADC Values and set Arduino Pins to a defined value.
///
void Rotor::initRotor()
{
if(AZIM)
{
//Pin Settings
pinMode(AZIM_RE_CW, OUTPUT);
pinMode(AZIM_RE_CCW, OUTPUT);
pinMode(AZIM_RE_BREAK, OUTPUT);
//All Relais Off
digitalWrite(AZIM_RE_CW, HIGH);
digitalWrite(AZIM_RE_CCW, HIGH);
digitalWrite(AZIM_RE_BREAK, HIGH);
//Initial Values
m_bAzimuthBreakReleased = 0;
m_bAzimuthCurrentlyRotating = 0;
m_bAzimuthShouldRotate = 0;
}
if(ELEV)
{
//Pin Settings
pinMode(ELEV_RE_UP, OUTPUT);
pinMode(ELEV_RE_DWN, OUTPUT);
pinMode(ELEV_RE_BREAK, OUTPUT);
//All Relais Off
digitalWrite(ELEV_RE_UP, HIGH);
digitalWrite(ELEV_RE_DWN, HIGH);
digitalWrite(ELEV_RE_BREAK, HIGH);
//Initial Values
m_bElevationBreakReleased = 0;
m_bElevationCurrentlyRotating = 0;
m_bElevationShouldRotate = 0;
}
}
///
/// @brief Does all the rotor movement stuff
///
void Rotor::doRotor()
{
int diff_elev, diff_azim;
readRotSensors();
if(AZIM && m_bAzimuthShouldRotate)
{
diff_azim = m_uActualAzimuth - m_uSetAzimuth;
if(m_bAzimuthCurrentlyRotating) {
if(abs(diff_azim) < AZIM_RES)
stopAzimuth();
else {
if(diff_azim > 0)
rotateCW();
else
rotateCCW();
}
}
else {
if(abs(diff_azim) > AZIM_SPAN) {
releaseBreakAzimuth();
if(diff_azim > 0)
rotateCW();
else
rotateCCW();
}
}
}
if(ELEV && m_bElevationShouldRotate)
{
diff_elev = m_uActualElevation - m_uSetElevation;
if(m_bElevationCurrentlyRotating) {
if(abs(diff_elev) < ELEV_RES)
stopElevation();
else {
if(diff_elev > 0)
rotateDown();
else
rotateUp();
}
}
else {
if(abs(diff_elev) > ELEV_SPAN) {
releaseBreakElevation();
if(diff_elev > 0)
rotateDown();
else
rotateUp();
}
}
}
if ( !m_bAzimuthCurrentlyRotating && !m_bElevationCurrentlyRotating ) {
tightenBreakAzimuth();
tightenBreakElevation();
}
azimuth_->doMotor();
elevation_->doMotor();
}
void Rotor::releaseBreakAzimuth()
{
m_bAzimuthBreakReleased = 1;
digitalWrite(AZIM_RE_BREAK, LOW);
azimuth_->releaseBreak();
}
void Rotor::releaseBreakElevation()
{
m_bElevationBreakReleased = 1;
digitalWrite(ELEV_RE_BREAK, LOW);
elevation_->releaseBreak();
}
void Rotor::tightenBreakAzimuth()
{
m_bAzimuthBreakReleased = 0;
digitalWrite(AZIM_RE_BREAK, HIGH);
azimuth_->tightenBreak();
}
void Rotor::tightenBreakElevation()
{
m_bElevationBreakReleased = 0;
digitalWrite(ELEV_RE_BREAK, HIGH);
elevation_->tightenBreak();
}
void Rotor::stopAzimuth()
{
digitalWrite(AZIM_RE_CW, HIGH);
digitalWrite(AZIM_RE_CCW, HIGH);
m_bAzimuthCurrentlyRotating = 0;
m_bAzimuthShouldRotate = 0;
azimuth_->stop();
}
void Rotor::stopElevation()
{
digitalWrite(ELEV_RE_UP, HIGH);
digitalWrite(ELEV_RE_DWN, HIGH);
m_bElevationCurrentlyRotating = 0;
m_bElevationShouldRotate = 0;
elevation_->stop();
}
Rotor::rotorError Rotor::rotateCW()
Motor::motorError Rotor::rotateCW()
{
if ( m_bAzimuthBreakReleased == 0 )
return Rotor::ROT_BREAK_NOT_RELEASED;
digitalWrite(AZIM_RE_CW, LOW);
digitalWrite(AZIM_RE_CCW, HIGH);
m_bAzimuthCurrentlyRotating = 1;
return Rotor::ROT_OK;
return azimuth_->rotateDown();
}
Rotor::rotorError Rotor::rotateCCW()
Motor::motorError Rotor::rotateCCW()
{
if ( m_bAzimuthBreakReleased == 0 )
return Rotor::ROT_BREAK_NOT_RELEASED;
digitalWrite(AZIM_RE_CW, HIGH);
digitalWrite(AZIM_RE_CCW, LOW);
m_bAzimuthCurrentlyRotating = 1;
return Rotor::ROT_OK;
return azimuth_->rotateUp();
}
Rotor::rotorError Rotor::rotateUp()
Motor::motorError Rotor::rotateUp()
{
if ( m_bElevationBreakReleased == 0 )
return Rotor::ROT_BREAK_NOT_RELEASED;
digitalWrite(ELEV_RE_UP, LOW);
digitalWrite(ELEV_RE_DWN, HIGH);
m_bElevationCurrentlyRotating = 1;
return Rotor::ROT_OK;
return elevation_->rotateUp();
}
Rotor::rotorError Rotor::rotateDown()
Motor::motorError Rotor::rotateDown()
{
if ( m_bElevationBreakReleased == 0 )
return Rotor::ROT_BREAK_NOT_RELEASED;
digitalWrite(ELEV_RE_UP, HIGH);
digitalWrite(ELEV_RE_DWN, LOW);
m_bElevationCurrentlyRotating = 1;
return Rotor::ROT_OK;
return elevation_->rotateDown();
}
///
/// @brief Serial Debug output Function
///
void Rotor::debugOut()
double Rotor::getAzimuth() const
{
//debug Out
readRotSensors();
Serial.println("-----------------");
Serial.print("Azim Value: ");
Serial.println(m_uActualAzimuth);
Serial.print("Elev Value: ");
Serial.println(m_uActualElevation);
return azimuth_->getPosition();
}
///
/// @brief Returns the actual Azimuth value in degree
///
/// @return uint16 actual Azimuth value in degree
///
uint16 Rotor::getActualAzimuth() const
double Rotor::getElevation() const
{
return m_uActualAzimuth;
return elevation_->getPosition();
}
///
/// @brief Returns the actual Elevation value in degree
///
/// @return actual Elevation value in degree
///
uint16 Rotor::getActualElevation() const
Motor::motorError Rotor::setAzimuth(double pAzimuth)
{
return m_uActualElevation;
return azimuth_->setPosition(pAzimuth);
}
///
/// @brief Sets the actual Azimuth value in degree and checks valid range
///
/// @param pAzimuth Azimuth value to be set
///
/// @return Error Code
///
Rotor::rotorError Rotor::setAzimuth(uint16 pAzimuth)
{
if(pAzimuth >= AZIM_MIN && pAzimuth <= AZIM_MAX)
{
m_uSetAzimuth = pAzimuth;
m_bAzimuthShouldRotate = 1;
return Rotor::ROT_OK;
}
else
{
return Rotor::ROT_VAL_OUT_OF_RANGE;
}
}
///
/// @brief Sets the actual Elevation value in degree and checks valid range
///
/// @param pElevation Elevation value to be set
///
/// @return Error Code
///
Rotor::rotorError Rotor::setElevation(uint16 pElevation)
{
if(pElevation >= ELEV_MIN && pElevation <= ELEV_MAX)
{
m_uSetElevation = pElevation;
m_bElevationShouldRotate = 1;
return Rotor::ROT_OK;
}
else
{
return Rotor::ROT_VAL_OUT_OF_RANGE;
}
}
///
/// @brief Reads out the Sensor values and stores them
///
/// @return Error Code
///
Rotor::rotorError Rotor::readRotSensors()
{
if(AZIM)
m_uActualAzimuth = (uint16)(readADC(AZIM_POT)/AZIM_DEG);
if(ELEV)
m_uActualElevation = (uint16)(readADC(ELEV_POT)/ELEV_DEG);
return Rotor::ROT_OK;