Commit 4c0b9205 authored by andz's avatar andz

Merge branch 'refactor_brake' into 'master'

Refactor brake and support stop/brake

See merge request !2
parents a42e2a62 c5e40cb3
#include "brake.h"
Brake::Brake(int brake_pin, int release_delay_in_ms)
{
last_release_ = 0;
brake_counter_ = 0;
brake_pin_ = brake_pin;
brake_tightened_ = true;
release_delay_in_ms_ = release_delay_in_ms;
//Pin Settings
pinMode(brake_pin_, OUTPUT);
//All Relais Off
digitalWrite(brake_pin_, HIGH);
}
void Brake::doBreak()
{
if(brake_counter_ == 0)
{
if(brake_tightened_ == false)
{
int diff = millis() - last_release_;
if((diff > release_delay_in_ms_) || (diff < 0))
{
digitalWrite(brake_pin_, HIGH);
brake_tightened_ = true;
}
}
}
else
{
digitalWrite(brake_pin_, LOW);
last_release_ = millis();
brake_tightened_ = false;
}
}
void Brake::skipDelay()
{
last_release_ = millis() - release_delay_in_ms_ - 1;
}
void Brake::release()
{
last_release_ = millis();
brake_counter_++;
}
void Brake::tighten()
{
brake_counter_--;
}
#ifndef BRAKE_H
#define BRAKE_H
#include "settings.h"
#include "Arduino.h"
class Brake
{
public:
Brake(int brake_pin, int release_delay_in_ms);
void doBreak();
void tighten();
void release();
void skipDelay();
private:
int last_release_;
int brake_counter_;
int brake_pin_;
int release_delay_in_ms_;
byte brake_tightened_;
};
#endif //BRAKE_H
......@@ -51,11 +51,11 @@ void Control::doControl()
Control::hamlibError Control::executeCommandString(String *command_string, Communicator *communicator)
{
String substring;
String response = "";
int lastPos = -1;
int newPos = 0;
int dst = 0;
while (newPos = command_string->indexOf(";", lastPos + 1)) {
while ((newPos = command_string->indexOf(";", lastPos + 1)) != -1) {
substring = command_string->substring(lastPos + 1, newPos);
lastPos = newPos;
......@@ -68,29 +68,46 @@ Control::hamlibError Control::executeCommandString(String *command_string, Commu
// 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 {
if(executeActiveCommandSubstring(substring) == HAMLIB_OK)
response += "1";
else
response += "0";
}
}
if(response != "")
communicator->sendMessage(response);
return HAMLIB_OK;
}
Control::hamlibError Control::executeActiveCommandSubstring(String substring)
{
Motor::motorError err;
float dst = 0.0;
if (substring.startsWith("setaz") || substring.startsWith("setel")) {
if( substring.length() < 7 )
return HAMLIB_NOK;
dst = substring.substring(5).toFloat();
if ( dst == 0 && substring.substring(5) != "0" )
return HAMLIB_NOK;
if(substring.startsWith("setaz"))
err = rotor_->setAzimuth(dst);
else
err = rotor_->setElevation(dst);
if(err == Motor::ROT_VAL_OUT_OF_RANGE)
return HAMLIB_NOK;
}
else if (substring == "stop") {
rotor_->stopAzimuth();
rotor_->stopElevation();
}
else if (substring == "brake") {
rotor_->stopAndBreakAzimuth();
rotor_->stopAndBreakElevation();
}
else
return HAMLIB_NOK;
return HAMLIB_OK;
}
......@@ -5,6 +5,7 @@
#include "rotor.h"
#include "serial_communicator.h"
#include "udp_communicator.h"
#include "motor.h"
class Control
{
......@@ -22,6 +23,7 @@ public:
private:
void doCommunicate();
hamlibError executeCommandString(String*, Communicator*);
hamlibError executeActiveCommandSubstring(String);
Rotor *rotor_;
Communicator *serial_communicator_;
......
#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)
Motor::Motor(double max, double min, int down_pin, int up_pin, Brake* brake, int sensor_pin, double degree, double resolution, double span)
{
max_ = max;
min_ = min;
span_ = span;
brake_ = brake;
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;
brake_tightened_ = true;
}
void Motor::doMotor()
......@@ -45,7 +44,7 @@ void Motor::doMotor()
Motor::motorError Motor::rotateUp()
{
releaseBreak();
releaseBrake();
digitalWrite(down_pin_, HIGH);
digitalWrite(up_pin_, LOW);
return Motor::ROT_OK;
......@@ -53,26 +52,32 @@ Motor::motorError Motor::rotateUp()
Motor::motorError Motor::rotateDown()
{
releaseBreak();
releaseBrake();
digitalWrite(up_pin_, HIGH);
digitalWrite(down_pin_, LOW);
return Motor::ROT_OK;
}
void Motor::releaseBreak()
void Motor::releaseBrake()
{
digitalWrite(break_pin_, LOW);
if(brake_tightened_) {
brake_->release();
brake_tightened_ = false;
}
}
void Motor::tightenBreak()
void Motor::tightenBrake()
{
digitalWrite(break_pin_, HIGH);
if(brake_tightened_ == false) {
brake_->tighten();
brake_tightened_ = true;
}
}
void Motor::stop()
{
should_rotate_ = false;
tightenBreak();
tightenBrake();
digitalWrite(down_pin_, HIGH);
digitalWrite(up_pin_, HIGH);
}
......
......@@ -2,6 +2,7 @@
#define MOTOR_H
#include "Arduino.h"
#include "brake.h"
class Motor
{
......@@ -11,27 +12,25 @@ public:
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);
Motor(double max, double min, int down_pin, int up_pin, Brake *brake, int sensor_pin, double degree, double resolution, double span);
void doMotor();
double getPosition() const;
void releaseBreak();
void tightenBreak();
void stop();
double getPosition() const;
motorError setPosition(double);
private:
void releaseBrake();
void tightenBrake();
motorError rotateUp();
motorError rotateDown();
private:
motorError readMotSensor();
double readADC(int);
Brake* brake_;
double max_;
double min_;
int down_pin_;
int up_pin_;
int break_pin_;
int sensor_pin_;
double position_;
double destination_;
......@@ -39,6 +38,7 @@ private:
double resolution_;
double span_;
byte should_rotate_;
byte brake_tightened_;
};
#endif //MOTOR_H
......@@ -2,34 +2,16 @@
Rotor::Rotor()
{
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);
brake_ = new Brake(AZIM_RE_BREAK, 10000);
azimuth_ = new Motor(AZIM_MAX, AZIM_MIN, AZIM_RE_CW, AZIM_RE_CCW, brake_, AZIM_POT, AZIM_DEG, AZIM_RES, AZIM_SPAN);
elevation_ = new Motor(ELEV_MAX, ELEV_MIN, ELEV_RE_DWN, ELEV_RE_UP, brake_, ELEV_POT, ELEV_DEG, ELEV_RES, ELEV_SPAN);
}
void Rotor::doRotor()
{
azimuth_->doMotor();
elevation_->doMotor();
}
void Rotor::releaseBreakAzimuth()
{
azimuth_->releaseBreak();
}
void Rotor::releaseBreakElevation()
{
elevation_->releaseBreak();
}
void Rotor::tightenBreakAzimuth()
{
azimuth_->tightenBreak();
}
void Rotor::tightenBreakElevation()
{
elevation_->tightenBreak();
brake_->doBreak();
}
void Rotor::stopAzimuth()
......@@ -42,24 +24,16 @@ void Rotor::stopElevation()
elevation_->stop();
}
Motor::motorError Rotor::rotateCW()
{
return azimuth_->rotateDown();
}
Motor::motorError Rotor::rotateCCW()
{
return azimuth_->rotateUp();
}
Motor::motorError Rotor::rotateUp()
void Rotor::stopAndBreakAzimuth()
{
return elevation_->rotateUp();
stopAzimuth();
brake_->skipDelay();
}
Motor::motorError Rotor::rotateDown()
void Rotor::stopAndBreakElevation()
{
return elevation_->rotateDown();
stopElevation();
brake_->skipDelay();
}
double Rotor::getAzimuth() const
......
......@@ -10,6 +10,7 @@
#include "settings.h"
#include "motor.h"
#include "brake.h"
class Rotor
{
......@@ -20,23 +21,17 @@ public:
double getAzimuth() const;
double getElevation() const;
void releaseBreakAzimuth();
void releaseBreakElevation();
void tightenBreakAzimuth();
void tightenBreakElevation();
void stopAzimuth();
void stopElevation();
void stopAndBreakAzimuth();
void stopAndBreakElevation();
Motor::motorError setAzimuth(double);
Motor::motorError setElevation(double);
Motor::motorError rotateCW();
Motor::motorError rotateCCW();
Motor::motorError rotateUp();
Motor::motorError rotateDown();
private:
Motor *azimuth_;
Motor *elevation_;
Brake *brake_;
};
#endif //ROTOR_H
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment