123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167 |
- #ifndef ACTUATOR_MODULE
- #define ACTUATOR_MODULE
- #include "Arduino.h"
- #include "definitions.h"
- struct Actuator
- {
- public:
- enum State {
- STILL,
- PUSH,
- PULL
- };
- private:
-
- PIN_TYPE motor_push_pin;
- PIN_TYPE motor_pull_pin;
- int max_distance_pwm;
- State current_state;
- bool change_required;
- GlobalState* global_state;
- public:
- CONST_PIN_TYPE ANALOUGE_PIN = 1;
- Actuator (PIN_TYPE motor_push_pin, PIN_TYPE motor_pull_pin,
- int max_distance_pwm, GlobalState* global_state)
- {
- this->current_state = STILL;
- this->motor_push_pin = motor_push_pin;
- this->motor_pull_pin = motor_pull_pin;
- this->max_distance_pwm = max_distance_pwm;
- this->global_state = global_state;
- }
- void setup()
- {
- this->global_state->actuator_at_max_push = false;
- // Serial.println("Setting up actiator");
- this->setState(PULL);
- // printState();
- while (this->current_state != STILL)
- {
- this->motorLoop();
- //Serial.print("Setup loop - current state: ");
- //Serial.println(this->current_state);
- }
- //Serial.println("Setup loop done");
- }
- void setMaxDistance(int max_distance_pwm)
- {
- this->max_distance_pwm = max_distance_pwm;
- }
- void printState()
- {
- Serial.print("Actuator current state: ");
- Serial.print(this->current_state);
- Serial.print(" - and PWM value: ");
- Serial.println(analogRead(ANALOUGE_PIN));
- }
- void setState(State state)
- {
- // Serial.print("Current state: ");
- // Serial.println(this->current_state);
- if (state != this->current_state)
- {
- this->current_state = state;
- this->change_required = true;
- }
- }
- int getMotorDist()
- {
- return analogRead(ANALOUGE_PIN);
- }
- void motorLoop()
- {
- // Serial.print("Motor loop - ");
- //printState();
- if (this->current_state == STILL)
- {
- return;
- }
-
- #define MIN_DISTANCE 20
- int sensor_value = this->getMotorDist();
- // if (sensor_value >= this->max_distance_pwm - 30) // Leave a bit of buffer
- if (sensor_value >= this->max_distance_pwm)
- {
- this->global_state->actuator_at_max_push = true;
- }
- else
- {
- this->global_state->actuator_at_max_push = false;
- if (sensor_value <= MIN_DISTANCE)
- {
- this->global_state->actuator_at_min_pull = true;
- }
- else
- {
- this->global_state->actuator_at_min_pull = false;
- }
- }
- if (this->current_state == PUSH)
- {
- if (sensor_value >= this->max_distance_pwm)
- {
- this->enableStill();
- this->current_state = STILL;
- this->change_required = false;
- }
-
- if (this->change_required)
- {
- this->enablePush();
- this->change_required = false;
- }
- }
- else if (this->current_state == PULL)
- {
- if (sensor_value <= MIN_DISTANCE) // Doesn't have to be completely retracted
- {
- this->enableStill();
- this->current_state = STILL;
- this->change_required = false;
- }
- if (this->change_required)
- {
- this->enablePull();
- this->change_required = false;
- }
- }
- }
- void enablePush()
- {
- digitalWrite(this->motor_push_pin, HIGH);
- digitalWrite(this->motor_pull_pin, LOW);
- }
- void enablePull()
- {
- digitalWrite(this->motor_push_pin, LOW);
- digitalWrite(this->motor_pull_pin, HIGH);
- }
- void enableStill()
- {
- digitalWrite(this->motor_push_pin, LOW);
- digitalWrite(this->motor_pull_pin, LOW);
- }
- };
- #endif
|