Implemented debugging commands:
- scroll positions - sensor values Implemented motor controls: - move to position - rewind/zero (tested on vertical scroll only) Untested: - timer for position control / detect scroll rip
This commit is contained in:
jpunkt
parent
ef34adf75c
commit
eeb7df97f7
2 changed files with 130 additions and 38 deletions
|
|
@ -19,6 +19,10 @@ enum Command {
|
|||
|
||||
RECORD = 'C',
|
||||
REWIND = 'R',
|
||||
|
||||
DEBUG_SCROLL = 'S',
|
||||
DEBUG_SENSORS = 'Z',
|
||||
|
||||
EOT = '\n'
|
||||
};
|
||||
|
||||
|
|
|
|||
164
src/main.cpp
164
src/main.cpp
|
|
@ -37,8 +37,8 @@
|
|||
// Horizontal sensors
|
||||
#define HORZ_END_OUTER 33
|
||||
#define HORZ_END_INNER 34
|
||||
#define HORZ_CNT_INNER 35
|
||||
#define HORZ_CNT_OUTER 36
|
||||
#define HORZ_CNT_INNER 36
|
||||
#define HORZ_CNT_OUTER 35
|
||||
|
||||
// Lights
|
||||
#define LED_FRONT 41
|
||||
|
|
@ -47,6 +47,11 @@
|
|||
#define LED_BACK 14
|
||||
#define LED_COUNT_BACK 72
|
||||
|
||||
/*-------- Constants --------*/
|
||||
|
||||
const int ENDSTOP_OVERRIDE = 8500; // time to ignore endstop when motor starts (x 10 us)
|
||||
const int SCROLL_ERROR_MS = 500; // if sensor values don't change in this time, the scroll has an error
|
||||
|
||||
/*-------- State Definitions --------*/
|
||||
|
||||
// States - implementations below loop()
|
||||
|
|
@ -91,6 +96,10 @@ bool handshake_complete;
|
|||
volatile int16_t hor_pos;
|
||||
volatile int16_t vert_pos;
|
||||
|
||||
// Last change on position counters in millis
|
||||
elapsedMillis hor_lastchange;
|
||||
elapsedMillis vert_lastchange;
|
||||
|
||||
/*-------- Objects --------*/
|
||||
// Motors
|
||||
Motor vert_up(VERT_UP_PWM, VERT_UP_AIN1, VERT_UP_AIN2);
|
||||
|
|
@ -133,8 +142,12 @@ int32_t count(int pinA, int pinB) {
|
|||
*
|
||||
*/
|
||||
void hor_count() {
|
||||
// TODO enable: if (!digitalRead(HORZ_END_OUTER))
|
||||
hor_pos -= count(HORZ_CNT_INNER, HORZ_CNT_OUTER);
|
||||
if (!digitalRead(HORZ_END_OUTER)) {
|
||||
hor_pos -= count(HORZ_CNT_INNER, HORZ_CNT_OUTER);
|
||||
} else {
|
||||
hor_pos = 0;
|
||||
}
|
||||
hor_lastchange = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -142,8 +155,12 @@ void hor_count() {
|
|||
*
|
||||
*/
|
||||
void vert_count() {
|
||||
// TODO enable: if (!digitalRead(VERT_END_OUTER))
|
||||
vert_pos -= count(VERT_CNT_INNER, VERT_CNT_OUTER);
|
||||
if (!digitalRead(VERT_END_INNER)) {
|
||||
vert_pos -= count(VERT_CNT_INNER, VERT_CNT_OUTER);
|
||||
} else {
|
||||
vert_pos = 0;
|
||||
}
|
||||
vert_lastchange = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -157,15 +174,11 @@ void blink_builtin(uint32_t on_interval, uint32_t off_interval) {
|
|||
if (blink_time >= on_interval) {
|
||||
digitalWrite(LED_BUILTIN, LOW);
|
||||
blink_time = blink_time - on_interval;
|
||||
// led_on = false;
|
||||
// Serial.println("Turn LED off");
|
||||
}
|
||||
} else {
|
||||
if (blink_time >= off_interval) {
|
||||
digitalWrite(LED_BUILTIN, HIGH);
|
||||
blink_time = blink_time - off_interval;
|
||||
// led_on = true;
|
||||
// Serial.println("Turn LED on");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -173,34 +186,45 @@ void blink_builtin(uint32_t on_interval, uint32_t off_interval) {
|
|||
/**
|
||||
* @brief Generic scroll zeroing code
|
||||
*
|
||||
* zero_pin | end_pin | Condition -> Action
|
||||
* -----------+-----------+---------------------------
|
||||
* 0 | 0 | scroll between start & end -> rewind
|
||||
* 0 | 1 | scroll at end -> rewind
|
||||
* 1 | 0 | scroll zeroed -> do nothing
|
||||
* 1 | 1 | scroll at start end-stop -> forward until end-stop free
|
||||
*
|
||||
* @param mot1 Motor in positive direction
|
||||
* @param mot2 Motor in negative direction
|
||||
* @param zero_pin Sensor pin where LOW enables count
|
||||
* @param end_pin Sensor pin attached to emergency stop (end-stop)
|
||||
*/
|
||||
void zero_motor(Motor &mot1, Motor &mot2, int zero_pin, int end_pin) {
|
||||
bool is_zero = digitalRead(end_pin) & !digitalRead(zero_pin);
|
||||
uint32_t end_time = millis() + 10000;
|
||||
|
||||
while (!is_zero & (millis() < end_time))
|
||||
{
|
||||
mot2.run(255, false);
|
||||
mot1.run(127, false);
|
||||
is_zero = digitalRead(zero_pin);
|
||||
}
|
||||
|
||||
delayMicroseconds(20);
|
||||
|
||||
end_time = millis() + 200;
|
||||
|
||||
while (digitalRead(zero_pin) & (millis() < end_time))
|
||||
{
|
||||
mot1.run(200, true);
|
||||
// elapsedMillis time = 0;
|
||||
if (!digitalRead(zero_pin)) {
|
||||
// rewind
|
||||
while (!digitalRead(zero_pin)) {
|
||||
mot2.run(255, false);
|
||||
mot1.run(127, false);
|
||||
}
|
||||
mot1.stop(false);
|
||||
delay(20); // TODO evaluate
|
||||
mot2.stop(false);
|
||||
delay(10);
|
||||
mot1.stop(true);
|
||||
mot2.stop(true);
|
||||
} else if (digitalRead(end_pin)) {
|
||||
// move forward
|
||||
while(digitalRead(zero_pin)) {
|
||||
mot1.run(127, true);
|
||||
mot2.run(55, true);
|
||||
}
|
||||
mot2.stop(false);
|
||||
delay(20);
|
||||
mot1.stop(false);
|
||||
delay(10);
|
||||
mot1.stop(true);
|
||||
mot2.stop(true);
|
||||
}
|
||||
|
||||
mot1.stop(false);
|
||||
mot2.stop(false);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -305,6 +329,23 @@ bool mot_control(Motor &mot1, Motor &mot2, volatile int16_t &pos, int16_t &aim)
|
|||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Generic check endstop and stop motor
|
||||
*
|
||||
* @param mot1
|
||||
* @param mot2
|
||||
* @param outer_pin
|
||||
*/
|
||||
bool mot_stop(Motor &mot1, Motor &mot2, int outer_pin) {
|
||||
if (digitalRead(outer_pin)) {
|
||||
mot1.stop(true);
|
||||
mot2.stop(true);
|
||||
Serial.printf("Motor stopped. \n");
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Generic serial command handler to drive scroll to position
|
||||
*
|
||||
|
|
@ -312,13 +353,20 @@ bool mot_control(Motor &mot1, Motor &mot2, volatile int16_t &pos, int16_t &aim)
|
|||
* @param mot2 Motor in negative direction
|
||||
* @param pos position variable
|
||||
*/
|
||||
void serial_motor(Motor &mot1, Motor &mot2, volatile int16_t &pos) {
|
||||
void serial_motor(Motor &mot1, Motor &mot2, volatile int16_t &pos, int end_pin) {
|
||||
int16_t inc = ssp.readInt16();
|
||||
ssp.readEot();
|
||||
|
||||
int16_t aim = pos + inc;
|
||||
int16_t c = 0;
|
||||
while (!mot_control(mot1, mot2, pos, aim)) {
|
||||
Serial.printf("aim = %i, pos = %i \n", aim, pos);
|
||||
if (c < ENDSTOP_OVERRIDE) {
|
||||
c++;
|
||||
} else {
|
||||
if (mot_stop(mot1, mot2, end_pin)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
delayMicroseconds(10);
|
||||
}
|
||||
}
|
||||
|
|
@ -368,7 +416,7 @@ void serial_frontlight() {
|
|||
*/
|
||||
void serial_motor_v() {
|
||||
Serial.println("Received MOTOR_V");
|
||||
serial_motor(vert_up, vert_down, vert_pos);
|
||||
serial_motor(vert_up, vert_down, vert_pos, VERT_END_OUTER);
|
||||
serial_received();
|
||||
}
|
||||
|
||||
|
|
@ -377,8 +425,8 @@ void serial_motor_v() {
|
|||
*
|
||||
*/
|
||||
void serial_motor_h() {
|
||||
Serial.println("Received MOTOR_V");
|
||||
serial_motor(horz_left, horz_right, hor_pos);
|
||||
Serial.println("Received MOTOR_H");
|
||||
serial_motor(horz_left, horz_right, hor_pos, HORZ_END_INNER);
|
||||
serial_received();
|
||||
}
|
||||
|
||||
|
|
@ -399,6 +447,7 @@ void serial_record() {
|
|||
void serial_rewind() {
|
||||
ssp.readEot();
|
||||
Serial.println("Received REWIND");
|
||||
zero_motor(vert_up, vert_down, VERT_END_INNER, VERT_END_OUTER);
|
||||
serial_received();
|
||||
}
|
||||
|
||||
|
|
@ -412,6 +461,36 @@ void serial_userinteract() {
|
|||
serial_received();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Serial command handler for debugging scroll positions. Logs to USB Serial
|
||||
*
|
||||
*/
|
||||
void serial_debug_pos() {
|
||||
ssp.readEot();
|
||||
u_int32_t hlt = hor_lastchange;
|
||||
u_int32_t vlt = vert_lastchange;
|
||||
Serial.printf("Scroll positions (last change): H=%d (%d), V=%d (%d) \n", hor_pos, hlt, vert_pos, vlt);
|
||||
serial_received();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Serial command handler for debugging sensors. Logs to USB Serial
|
||||
*
|
||||
*/
|
||||
void serial_debug_sens() {
|
||||
ssp.readEot();
|
||||
Serial.printf("VERT_END_OUTER = %d \nVERT_END_INNER = %d \nVERT_CNT_OUTER = %d \nVERT_CNT_INNER = %d \nHORZ_END_OUTER = %d \nHORZ_END_INNER = %d \nHORZ_CNT_INNER = %d \nHORZ_CNT_OUTER = %d \n",
|
||||
digitalRead(VERT_END_OUTER),
|
||||
digitalRead(VERT_END_INNER),
|
||||
digitalRead(VERT_CNT_OUTER),
|
||||
digitalRead(VERT_CNT_INNER),
|
||||
digitalRead(HORZ_END_OUTER),
|
||||
digitalRead(HORZ_END_INNER),
|
||||
digitalRead(HORZ_CNT_INNER),
|
||||
digitalRead(HORZ_CNT_OUTER));
|
||||
serial_received();
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(115200);
|
||||
|
|
@ -421,6 +500,8 @@ void setup() {
|
|||
|
||||
hor_pos = 0;
|
||||
vert_pos = 0;
|
||||
hor_lastchange = 0;
|
||||
vert_lastchange = 0;
|
||||
|
||||
blink_time = 0;
|
||||
|
||||
|
|
@ -438,11 +519,19 @@ void setup() {
|
|||
pinMode(HORZ_CNT_OUTER, INPUT);
|
||||
pinMode(VERT_CNT_INNER, INPUT);
|
||||
pinMode(VERT_CNT_OUTER, INPUT);
|
||||
pinMode(HORZ_END_INNER, INPUT);
|
||||
pinMode(HORZ_END_OUTER, INPUT);
|
||||
pinMode(VERT_END_INNER, INPUT);
|
||||
pinMode(VERT_END_OUTER, INPUT);
|
||||
|
||||
digitalWrite(HORZ_CNT_INNER, LOW);
|
||||
digitalWrite(HORZ_CNT_OUTER, LOW);
|
||||
digitalWrite(VERT_CNT_INNER, LOW);
|
||||
digitalWrite(VERT_CNT_OUTER, LOW);
|
||||
digitalWrite(HORZ_END_INNER, LOW);
|
||||
digitalWrite(HORZ_END_OUTER, LOW);
|
||||
digitalWrite(VERT_END_INNER, LOW);
|
||||
digitalWrite(VERT_END_OUTER, LOW);
|
||||
|
||||
led_back.begin();
|
||||
led_back.show();
|
||||
|
|
@ -459,6 +548,8 @@ void setup() {
|
|||
ssp.registerCommand(MOTOR_V, serial_motor_v);
|
||||
ssp.registerCommand(RECORD, serial_record);
|
||||
ssp.registerCommand(REWIND, serial_rewind);
|
||||
ssp.registerCommand(DEBUG_SCROLL, serial_debug_pos);
|
||||
ssp.registerCommand(DEBUG_SENSORS, serial_debug_sens);
|
||||
ssp.registerCommand(USER_INTERACT, serial_userinteract);
|
||||
|
||||
S00->addTransition(transition_post_zero, S10);
|
||||
|
|
@ -470,7 +561,6 @@ void setup() {
|
|||
void loop() {
|
||||
// Just run the state machine
|
||||
sm.run();
|
||||
// blink_builtin(WAIT_ON_MS, WAIT_OFF_MS);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -502,9 +592,7 @@ void state_zero() {
|
|||
void state_init_callbacks() {
|
||||
Serial.println("State Initialize Callbacks.");
|
||||
attachInterrupt(digitalPinToInterrupt(HORZ_CNT_INNER), hor_count, CHANGE);
|
||||
// attachInterrupt(digitalPinToInterrupt(HORZ_CNT_OUTER), hor_count, CHANGE);
|
||||
attachInterrupt(digitalPinToInterrupt(VERT_CNT_INNER), vert_count, CHANGE);
|
||||
// attachInterrupt(digitalPinToInterrupt(VERT_CNT_OUTER), vert_count, CHANGE);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in a new issue