johannes.payr/pizzabox-teensy
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Preliminary motor tests (success).

Browse source
This commit is contained in:
jpunkt 2021-12-15 19:20:04 +01:00
parent ae9c6451a9
commit 41dead1da1
2 changed files with 28 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
src/Motor.cpp
View file

@ -10,8 +10,11 @@ Motor::Motor(uint8_t pwm, uint8_t in1, uint8_t in2)
void Motor::setup() void Motor::setup()
{ {
pinMode(pwm_pin, OUTPUT); pinMode(pwm_pin, OUTPUT);
analogWrite(pwm_pin, 0);
pinMode(in1_pin, OUTPUT); pinMode(in1_pin, OUTPUT);
digitalWrite(in1_pin, LOW);
pinMode(in2_pin, OUTPUT); pinMode(in2_pin, OUTPUT);
digitalWrite(in2_pin, LOW);
} }
void Motor::run(uint8_t speed, bool forward) void Motor::run(uint8_t speed, bool forward)

30
src/main.cpp
View file

@ -36,14 +36,19 @@
volatile int32_t hor_pos; volatile int32_t hor_pos;
volatile int32_t vert_pos; volatile int32_t vert_pos;
int32_t hor_aim;
int32_t vert_aim;
bool up;
Motor vert_up(VERT_UP_PWM, VERT_UP_AIN1, VERT_UP_AIN2); Motor vert_up(VERT_UP_PWM, VERT_UP_AIN1, VERT_UP_AIN2);
Motor vert_down(VERT_DOWN_PWM, VERT_DOWN_AIN1, VERT_DOWN_AIN2); Motor vert_down(VERT_DOWN_PWM, VERT_DOWN_AIN1, VERT_DOWN_AIN2);
Motor horz_left(HORZ_LEFT_PWM, HORZ_LEFT_AIN1, HORZ_LEFT_AIN2); Motor horz_left(HORZ_LEFT_PWM, HORZ_LEFT_AIN1, HORZ_LEFT_AIN2);
Motor horz_right(HORZ_RIGHT_PWM, HORZ_RIGHT_AIN1, HORZ_RIGHT_AIN2); Motor horz_right(HORZ_RIGHT_PWM, HORZ_RIGHT_AIN1, HORZ_RIGHT_AIN2);
int32_t count(int pinA, int pinB) { int32_t count(int pinA, int pinB) {
if (digitalRead(pinA)) return digitalRead(pinB) ? 1 : -1; if (digitalRead(pinA)) return digitalRead(pinB) ? -1 : 1;
else return digitalRead(pinB) ? -1 : 1; else return digitalRead(pinB) ? 1 : -1;
} }
void hor_count() { void hor_count() {
@ -62,6 +67,9 @@ void setup() {
hor_pos = 0; hor_pos = 0;
vert_pos = 0; vert_pos = 0;
hor_aim = 0;
vert_aim = -20;
vert_up.setup(); vert_up.setup();
vert_down.setup(); vert_down.setup();
vert_up.stop(true); vert_up.stop(true);
@ -89,9 +97,21 @@ void setup() {
} }
void loop() { void loop() {
// Serial.printf("hor_count1: %i | \t hor_count2: %i | \t vert_count1: %i | \t vert_count2: %i \n", digitalRead(HORZ_CNT_INNER), digitalRead(HORZ_CNT_OUTER), digitalRead(VERT_CNT_INNER), digitalRead(VERT_CNT_OUTER));
Serial.printf("hor_count %i | \t vert_count %i \n", hor_pos, vert_pos); if (vert_pos < vert_aim) {
vert_down.run(255, false);
vert_up.run(127, false);
} else if (vert_pos > vert_aim) {
vert_up.run(255, true);
vert_down.run(127, true);
} else {
vert_up.stop(false);
vert_down.stop(false);
vert_aim = (vert_aim == 50) ? 0 : 50;
delay(100);
}
delay(500); Serial.printf("hor_count %i | \t vert_count %i | \t vert_aim %i \n", hor_pos, vert_pos, vert_aim);
delayMicroseconds(10);
} }