-
Notifications
You must be signed in to change notification settings - Fork 0
/
cattusmachina.ino
148 lines (131 loc) · 3.36 KB
/
cattusmachina.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
#include <Servo.h>
#include <Metro.h>
//#define Serial Serial1
#include <ResponsiveAnalogRead.h>
#include "Motors.h"
#include <Bounce.h>
#include <stdarg.h>
void p(char *fmt, ... ){
char buf[128]; // resulting string limited to 128 chars
va_list args;
va_start (args, fmt );
vsnprintf(buf, 128, fmt, args);
va_end (args);
Serial.println(buf);
Serial1.println(buf);
}
void p(const __FlashStringHelper *fmt, ... ){
char buf[128]; // resulting string limited to 128 chars
va_list args;
va_start (args, fmt);
#ifdef __AVR__
vsnprintf_P(buf, sizeof(buf), (const char *)fmt, args); // progmem for AVR
#else
vsnprintf(buf, sizeof(buf), (const char *)fmt, args); // for the rest of the world
#endif
va_end(args);
Serial.println(buf);
Serial1.println(buf);
}
#define BUMPER_BUTTON 14
Bounce bumper = Bounce(BUMPER_BUTTON, 5);
#define DIST_IN_L 16
ResponsiveAnalogRead dist_l_reader(DIST_IN_L, true);
int dist_l = 0;
#define DIST_IN_R 15
ResponsiveAnalogRead dist_r_reader(DIST_IN_R, true);
int dist_r = 0;
#define HAND_LEFT 10
#define HAND_LEFT_ZERO 0
#define HAND_LEFT_MAX 180
Servo handLeft;
int handLeftDir = HAND_LEFT_ZERO;
#define HAND_RIGHT 9
#define HAND_RIGHT_ZERO 180
#define HAND_RIGHT_MAX 30
Servo handRight;
int handRightDir = HAND_RIGHT_ZERO;
Metro debug_out = Metro(10000);
Motors motors = Motors();
int speed = 0;
int speedChange = 1;
#define RoboState uint8_t
#define StateInitial 0
#define StateGoingForward 1
#define StateHitWall 2
#define StateTurningLeft 3
#define StateTurningRight 4
RoboState currentState = StateInitial;
int prevTime = 0;
int deltaTime = 0;
int timeInState = 0;
void changeState(RoboState state) {
currentState = state;
timeInState = 0;
}
void setup() {
// initialize the digital pin as an output.
pinMode(13, OUTPUT);
digitalWrite(13, LOW);
delay(100);
digitalWrite(13, HIGH);
handRight.attach(HAND_RIGHT, 450, 2400);
handLeft.attach(HAND_LEFT, 490, 2520);
pinMode(BUMPER_BUTTON, INPUT_PULLUP);
motors.Attach();
Serial.begin(115200);
}
// the loop routine runs over and over again forever:
void loop() {
dist_l_reader.update();
dist_r_reader.update();
dist_l = dist_l_reader.getValue();
dist_r = dist_r_reader.getValue();
bumper.update();
const int bumperHit = !bumper.read();
switch (currentState) {
default:
case StateInitial: {
speed = 0;
if (bumperHit) {
delay(1000);
changeState(StateGoingForward);
}
break;
}
case StateGoingForward: {
handLeftDir = HAND_LEFT_ZERO;
handRightDir = HAND_RIGHT_ZERO;
speed = 255;
if (bumperHit) {
speed = 0;
changeState(dist_l > dist_r ? StateTurningRight : StateTurningLeft);
}
break;
}
case StateTurningRight: {
handLeftDir = HAND_LEFT_MAX;
speed = timeInState < 200 ? -1 : 0;
if (timeInState > 500) {
changeState(StateGoingForward);
}
break;
}
case StateTurningLeft: {
handRightDir = HAND_RIGHT_MAX;
speed = timeInState < 200 ? -1 : 0;
if (timeInState > 500) {
changeState(StateGoingForward);
}
break;
}
}
Serial.println(timeInState);
handLeft.write(handLeftDir);
handRight.write(handRightDir);
motors.SetSpeeds(speed);
const uint currentTime = millis();
deltaTime = currentTime - prevTime;
timeInState += deltaTime;
prevTime = currentTime;
}