171 lines
3.8 KiB
Plaintext
171 lines
3.8 KiB
Plaintext
|
|
#define m1 9
|
|
#define g1 2
|
|
#define m2 10
|
|
#define g2 3
|
|
#define m3 11
|
|
#define fc1 18
|
|
#define fc2 19
|
|
|
|
boolean st1 = 0;
|
|
boolean st2 = 1;
|
|
|
|
int ejex_med = 0;
|
|
int ejey_med = 0;
|
|
|
|
#define FREQ 1 // How many interrupts occur before the serial commands are read
|
|
#if FREQ > 1
|
|
byte int_counter = 0;
|
|
#endif
|
|
|
|
ISR(TIMER2_OVF_vect) {
|
|
sei(); //Reenable global interrupts, otherwise serial commands will get dropped
|
|
#if FREQ > 1
|
|
if(++int_counter == FREQ){ // Only do this once every FREQ-th interrupt
|
|
int_counter = 0;
|
|
#endif //FREQ
|
|
|
|
st1=!st1;
|
|
// st2=!st2;
|
|
// digitalWrite(7, st1);
|
|
// digitalWrite(5, st2);
|
|
if (st1==HIGH)
|
|
{
|
|
PORTD |= B00110000;
|
|
PORTD &= B00111111;
|
|
}
|
|
else
|
|
{
|
|
PORTD |= B11000000;
|
|
PORTD &= B11001111;
|
|
}
|
|
|
|
#if FREQ > 1
|
|
}
|
|
#endif //FREQ
|
|
}
|
|
|
|
void setup() {
|
|
pinMode(g1, OUTPUT);
|
|
pinMode(g2, OUTPUT);
|
|
pinMode(m1, OUTPUT);
|
|
pinMode(m2, OUTPUT);
|
|
pinMode(m3, OUTPUT);
|
|
pinMode(fc1, INPUT);
|
|
pinMode(fc2, INPUT);
|
|
/*pinMode(7, OUTPUT);
|
|
pinMode(5, OUTPUT);*/
|
|
|
|
DDRD |= B11110000;
|
|
PORTD |= B00110000;
|
|
PORTD &= B00111111;
|
|
|
|
digitalWrite(g1, LOW);
|
|
digitalWrite(g2, LOW);
|
|
digitalWrite(fc1, HIGH);
|
|
digitalWrite(fc2, HIGH);
|
|
digitalWrite(m1, 0);
|
|
digitalWrite(m2, 0);
|
|
digitalWrite(m3, 0);
|
|
|
|
ejey_med=analogRead(1);
|
|
ejex_med=analogRead(0);
|
|
|
|
/*010 (2MHz) with FREQ set to 10 or less seem to work reasonably well
|
|
Trying to use 011 (500kHz), to eliminate the if statement needed for FREQ
|
|
Might even work as slow as 100 (250kHz).
|
|
*/
|
|
TCCR2A = 0;
|
|
TCCR2B = 0<<CS22 | 1<<CS21 | 0<<CS20;
|
|
|
|
//Timer2 Overflow Interrupt Enable
|
|
TIMSK2 = 1<<TOIE2;
|
|
}
|
|
|
|
void loop() {
|
|
|
|
if (analogRead(1)>=(ejey_med + 200))
|
|
{
|
|
if (digitalRead(fc2)&& digitalRead(fc1))
|
|
{
|
|
motor2(0,1);
|
|
motor1(1,1);
|
|
}
|
|
else if (!digitalRead(fc2)&& !digitalRead(fc1))
|
|
{
|
|
motor2(0,1);
|
|
motor1(1,1);
|
|
delay(1000);
|
|
}
|
|
else if (!digitalRead(fc2)&& digitalRead(fc1))
|
|
{
|
|
motor2(0,0);
|
|
motor1(1,1);
|
|
}
|
|
else if (digitalRead(fc2)&& !digitalRead(fc1))
|
|
{
|
|
motor2(0,1);
|
|
motor1(1,0);
|
|
}
|
|
digitalWrite(m3, HIGH);
|
|
}
|
|
else if (analogRead(1)<(ejey_med - 200))
|
|
{
|
|
if (digitalRead(fc2)&& digitalRead(fc1))
|
|
{
|
|
motor2(1,1);
|
|
motor1(0,1);
|
|
}
|
|
else if (!digitalRead(fc2)&& !digitalRead(fc1))
|
|
{
|
|
motor2(1,1);
|
|
motor1(0,1);
|
|
delay(1000);
|
|
}
|
|
else if (!digitalRead(fc2)&& digitalRead(fc1))
|
|
{
|
|
motor2(1,0);
|
|
motor1(0,1);
|
|
}
|
|
else if (digitalRead(fc2)&& !digitalRead(fc1))
|
|
{
|
|
motor2(1,1);
|
|
motor1(0,0);
|
|
}
|
|
digitalWrite(m3, HIGH);
|
|
}
|
|
else if (analogRead(0)>=(ejex_med + 200))
|
|
{
|
|
motor2(1,1);
|
|
motor1(1,1);
|
|
}
|
|
else if (analogRead(0)<(ejex_med - 200))
|
|
{
|
|
motor2(0,1);
|
|
motor1(0,1);
|
|
}
|
|
else
|
|
{
|
|
motor2(0,0);
|
|
motor1(0,0);
|
|
digitalWrite(m3, 0);
|
|
}
|
|
|
|
/*if (analogRead(1)>=512) motor2(1,map(analogRead(1),512,1023,0,255));
|
|
else motor2(0,map(analogRead(1),0,511,255,0));*/
|
|
|
|
/*while (digitalRead(fc2)) motor2(0,255);
|
|
motor2(0,0);*/
|
|
|
|
}
|
|
|
|
void motor1 (boolean sent, int vel) {
|
|
digitalWrite(g1, sent);
|
|
digitalWrite(m1, vel);
|
|
}
|
|
|
|
void motor2 (boolean sent, int vel) {
|
|
digitalWrite(g2, sent);
|
|
digitalWrite(m2, vel);
|
|
}
|