815 lines
22 KiB
C++
815 lines
22 KiB
C++
int G[5] = { 33, 31, 29, 27, 25};
|
|
boolean fc[40] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
|
unsigned long time_costilla[10] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
|
|
unsigned long time_final_costilla[10] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
|
|
unsigned long time_aleta[8] = { 0, 0, 0, 0, 0, 0 };
|
|
unsigned long time_final_aleta[8] = { 0, 0, 0, 0, 0, 0 };
|
|
unsigned long time_giro = 0;
|
|
unsigned long time_final_giro = 0;
|
|
unsigned long time_sensor = 0;
|
|
|
|
|
|
#define A 39
|
|
#define B 37
|
|
#define C 35
|
|
|
|
#define PWM0 2
|
|
#define PWM1 6
|
|
#define PWM2 5
|
|
#define PWM3 4
|
|
#define PWM4 3
|
|
#define TIME_STOP 1200000
|
|
|
|
#define UP 15
|
|
#define DOWN 14
|
|
#define STOP 0
|
|
|
|
int latchPin0 = 7;
|
|
int clockPin0 = 8;
|
|
int dataPin0 = 9;
|
|
|
|
int latchPin1 = 12;
|
|
int clockPin1 = 11;
|
|
int dataPin1 = 10;
|
|
|
|
byte val_shift0[3] = { B10101010, B01010100, B00101010};
|
|
|
|
void shift0Write(int pin, boolean state)
|
|
{
|
|
if (pin < 24)
|
|
{
|
|
int var = pin / 8;
|
|
pin = pin - 8 * var;
|
|
bitWrite(val_shift0[var], pin, state);
|
|
digitalWrite(latchPin0, LOW);
|
|
for (int i = 3; i >= 0; i--) shiftOut(dataPin0, clockPin0, MSBFIRST, val_shift0[i]);
|
|
digitalWrite(latchPin0, HIGH);
|
|
}
|
|
}
|
|
|
|
void resetshift0()
|
|
{
|
|
digitalWrite(latchPin0, LOW);
|
|
for (int i = 3; i >= 0; i--) shiftOut(dataPin0, clockPin0, MSBFIRST, val_shift0[i]);
|
|
digitalWrite(latchPin0, HIGH);
|
|
analogWrite(PWM0, 0);
|
|
}
|
|
|
|
byte val_shift1[5] = { 0x00, 0x00, 0x00};
|
|
void shift1Write(int pin, boolean state)
|
|
{
|
|
if (pin < 24)
|
|
{
|
|
int var = pin / 8;
|
|
pin = pin - 8 * var;
|
|
bitWrite(val_shift1[var], pin, state);
|
|
digitalWrite(latchPin1, LOW);
|
|
for (int i = 3; i >= 0; i--) shiftOut(dataPin1, clockPin1, MSBFIRST, val_shift1[i]);
|
|
digitalWrite(latchPin1, HIGH);
|
|
}
|
|
}
|
|
|
|
void resetshift1()
|
|
{
|
|
digitalWrite(latchPin1, LOW);
|
|
for (int i = 3; i >= 0; i--) shiftOut(dataPin1, clockPin1, MSBFIRST, 0x00);
|
|
digitalWrite(latchPin1, HIGH);
|
|
}
|
|
|
|
void selgrup(int var)
|
|
{
|
|
switch (var)
|
|
{
|
|
case 3:
|
|
digitalWrite(A, HIGH);
|
|
digitalWrite(B, HIGH);
|
|
digitalWrite(C, LOW);
|
|
break;
|
|
case 0:
|
|
digitalWrite(A, LOW);
|
|
digitalWrite(B, LOW);
|
|
digitalWrite(C, LOW);
|
|
break;
|
|
case 1:
|
|
digitalWrite(A, HIGH);
|
|
digitalWrite(B, LOW);
|
|
digitalWrite(C, LOW);
|
|
break;
|
|
case 2:
|
|
digitalWrite(A, LOW);
|
|
digitalWrite(B, HIGH);
|
|
digitalWrite(C, LOW);
|
|
break;
|
|
case 4:
|
|
digitalWrite(A, LOW);
|
|
digitalWrite(B, LOW);
|
|
digitalWrite(C, HIGH);
|
|
break;
|
|
case 6:
|
|
digitalWrite(A, LOW);
|
|
digitalWrite(B, HIGH);
|
|
digitalWrite(C, HIGH);
|
|
break;
|
|
case 7:
|
|
digitalWrite(A, HIGH);
|
|
digitalWrite(B, HIGH);
|
|
digitalWrite(C, HIGH);
|
|
break;
|
|
case 5:
|
|
digitalWrite(A, HIGH);
|
|
digitalWrite(B, LOW);
|
|
digitalWrite(C, HIGH);
|
|
break;
|
|
}
|
|
}
|
|
|
|
boolean fcread(int pin)
|
|
{
|
|
switch (pin)
|
|
{
|
|
case 0:
|
|
selgrup(3);
|
|
delay(1);
|
|
|
|
if (!digitalRead(33))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(33))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 1:
|
|
selgrup(0);
|
|
delay(1);
|
|
|
|
if (!digitalRead(33))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(33))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 2:
|
|
selgrup(1);
|
|
delay(1);
|
|
|
|
if (!digitalRead(33))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(33))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 3:
|
|
selgrup(2);
|
|
delay(1);
|
|
|
|
if (!digitalRead(33))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(33))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 4:
|
|
selgrup(4);
|
|
delay(1);
|
|
|
|
if (!digitalRead(33))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(33))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 5:
|
|
selgrup(6);
|
|
delay(1);
|
|
|
|
if (!digitalRead(33))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(33))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 6:
|
|
selgrup(7);
|
|
delay(1);
|
|
|
|
if (!digitalRead(33))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(33))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 7:
|
|
selgrup(5);
|
|
delay(1);
|
|
|
|
if (!digitalRead(33))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(33))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 8:
|
|
selgrup(3);
|
|
delay(1);
|
|
|
|
if (!digitalRead(31))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(31))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 9:
|
|
selgrup(0);
|
|
delay(1);
|
|
|
|
if (!digitalRead(31))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(31))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 10:
|
|
selgrup(1);
|
|
delay(1);
|
|
|
|
if (!digitalRead(31))
|
|
{
|
|
unsigned long time_count=millis();
|
|
while ((!digitalRead(31))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 11:
|
|
selgrup(2);
|
|
delay(1);
|
|
if (!digitalRead(31))
|
|
{
|
|
unsigned long time_count = millis();
|
|
while ((!digitalRead(31))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
case 12:
|
|
selgrup(4);
|
|
delay(1);
|
|
if (!digitalRead(31))
|
|
{
|
|
unsigned long time_count = millis();
|
|
while ((!digitalRead(31))&&((millis()-time_count)<100));
|
|
if ((millis()-time_count)>=100) return false;
|
|
return true;
|
|
}
|
|
break;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
int flag_dir_c[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
|
int dir[10] = { 2, 4, 6, 9, 11, 13, 15, 18, 20, 22};
|
|
int en_c[10] = { 1, 3, 5, 7, 10, 12, 14, 17, 19, 21};
|
|
|
|
void costilla(int num, int value, unsigned long timemax)
|
|
{
|
|
time_final_costilla[num] = timemax;
|
|
time_costilla[num] = millis();
|
|
if (value > 0) flag_dir_c[num] = 1;
|
|
else if (value < 0) flag_dir_c[num] = -1;
|
|
else flag_dir_c[num] = 0;
|
|
|
|
if (num < 10)
|
|
{
|
|
if (value != 0)
|
|
{
|
|
shift0Write(en_c[num], false);
|
|
analogWrite(PWM0, abs(value));
|
|
}
|
|
else shift0Write(en_c[num], true);
|
|
if (value > 0) shift0Write(dir[num], true);
|
|
else shift0Write(dir[num], false);
|
|
}
|
|
}
|
|
|
|
int flag_dir[6] = { 0, 0, 0, 0, 0, 0 };
|
|
int dirA[6] = { 21, 18, 10, 13, 2, 5 };
|
|
int dirB[6] = { 20, 19, 11, 12, 3, 4 };
|
|
int en_a[6] = { 22, 17, 9, 14, 1, 6 };
|
|
int PWM[6] = { PWM3, PWM4, PWM2, PWM1, PWM2, PWM1 };
|
|
|
|
|
|
void motor(int num, int value, unsigned long timemax)
|
|
{
|
|
time_final_aleta[num] = timemax;
|
|
time_aleta[num] = millis();
|
|
if (value > 0) flag_dir[num] = 1;
|
|
else if (value < 0) flag_dir[num] = -1;
|
|
else flag_dir[num] = 0;
|
|
|
|
if (num < 6)
|
|
{
|
|
if (value != 0)
|
|
{
|
|
shift1Write(en_a[num], true);
|
|
analogWrite(PWM[num], abs(value));
|
|
}
|
|
else shift1Write(en_a[num], false);
|
|
if (value > 0) {
|
|
shift1Write(dirA[num], true);
|
|
shift1Write(dirB[num], false);
|
|
}
|
|
else {
|
|
shift1Write(dirA[num], false);
|
|
shift1Write(dirB[num], true);
|
|
}
|
|
}
|
|
}
|
|
|
|
void aleta_izquierda(int Speed, unsigned long timemax)
|
|
{
|
|
motor(0, Speed, timemax);
|
|
}
|
|
|
|
void aleta_derecha(int Speed, unsigned long timemax)
|
|
{
|
|
motor(1, Speed, timemax);
|
|
}
|
|
|
|
void aleta_izquierda_giro(int Speed, unsigned long timemax)
|
|
{
|
|
motor(2, Speed, timemax);
|
|
}
|
|
|
|
void aleta_derecha_giro(int Speed, unsigned long timemax)
|
|
{
|
|
motor(3, Speed, timemax);
|
|
}
|
|
|
|
void cola(int Speed, unsigned long timemax)
|
|
{
|
|
motor(4, Speed, timemax);
|
|
}
|
|
|
|
int flag_dir_g = 0;
|
|
|
|
void giro_cuerpo(int mode, unsigned long timemax)
|
|
{
|
|
time_final_giro = timemax;
|
|
time_giro = millis();
|
|
if (mode == UP)
|
|
{
|
|
flag_dir_g = 1;
|
|
digitalWrite(UP, HIGH);
|
|
digitalWrite(DOWN, LOW);
|
|
}
|
|
else if (mode == DOWN)
|
|
{
|
|
flag_dir_g = -1;
|
|
digitalWrite(UP, LOW);
|
|
digitalWrite(DOWN, HIGH);
|
|
}
|
|
else
|
|
{
|
|
flag_dir_g = 0;
|
|
digitalWrite(UP, LOW);
|
|
digitalWrite(DOWN, LOW);
|
|
}
|
|
|
|
}
|
|
int flag_costilla[10];
|
|
int flag_aleta[6];
|
|
int flag_giro;
|
|
int fc_aleta_up[6] = {2, 0, 4, 7, 8, 10};
|
|
int fc_aleta_down[6] = {3, 1, 5, 6, 9, 11};
|
|
|
|
void control()
|
|
{
|
|
for (int i = 0; i < 6; i++) flag_aleta[i] = 0;
|
|
for (int i = 0; i < 10; i++) flag_costilla[i] = 0;
|
|
flag_giro = 0;
|
|
while (((!flag_aleta[0]) || (!flag_aleta[1]) || (!flag_aleta[2]) || (!flag_aleta[3]) || (!flag_aleta[4]) || (!flag_aleta[5]))
|
|
||((!flag_costilla[0]) || (!flag_costilla[1]) || (!flag_costilla[2]) || (!flag_costilla[3]) || (!flag_costilla[4]) || (!flag_costilla[5]) || (!flag_costilla[6]) || (!flag_costilla[7]) || (!flag_costilla[8]) || (!flag_costilla[9]))||(!flag_giro))
|
|
{
|
|
for (int num = 0; num < 6; num++)
|
|
{
|
|
if (((millis() - time_aleta[num]) >= time_final_aleta[num]) || (!fcread(fc_aleta_up[num]) && (flag_dir[num] == 1)) || (!fcread(fc_aleta_down[num]) && (flag_dir[num] == -1)))
|
|
{
|
|
flag_aleta[num] = 1;
|
|
if (flag_dir[num] != 0)//&&((flag_dir[2] == 0)))
|
|
motor(num, 0, 0);
|
|
}
|
|
}
|
|
for (int num = 0; num < 10; num++)
|
|
{
|
|
if ((millis() - time_costilla[num]) >= time_final_costilla[num])
|
|
{
|
|
flag_costilla[num] = 1;
|
|
if (flag_dir_c[num] != 0)
|
|
costilla(num, 0, 0);
|
|
}
|
|
}
|
|
if (((millis() - time_giro) >= time_final_giro)||((flag_dir_g==1)&&(!fcread(11)))||((flag_dir_g==-1)&&(!fcread(10))))
|
|
{
|
|
flag_giro = 1;
|
|
if (flag_dir_g != 0)
|
|
{
|
|
giro_cuerpo(STOP,0);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
void reset_costillas()
|
|
{
|
|
for (int i = 0; i < 10; i++) costilla(i, -255, 15000);
|
|
control();
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(57600);
|
|
for (int i = 0; i < 5; i++)
|
|
{
|
|
pinMode(G[i], INPUT);
|
|
digitalWrite(G[i], HIGH);
|
|
}
|
|
pinMode(PWM0, OUTPUT);
|
|
pinMode(PWM1, OUTPUT);
|
|
pinMode(PWM2, OUTPUT);
|
|
pinMode(PWM3, OUTPUT);
|
|
pinMode(PWM4, OUTPUT);
|
|
pinMode(UP, OUTPUT);
|
|
pinMode(DOWN, OUTPUT);
|
|
pinMode(A, OUTPUT);
|
|
pinMode(B, OUTPUT);
|
|
pinMode(C, OUTPUT);
|
|
digitalWrite(A, LOW);
|
|
digitalWrite(B, LOW);
|
|
digitalWrite(C, LOW);
|
|
digitalWrite(UP, LOW);
|
|
digitalWrite(DOWN, LOW);
|
|
analogWrite(PWM0, 0);
|
|
analogWrite(PWM1, 0);
|
|
analogWrite(PWM2, 0);
|
|
analogWrite(PWM3, 0);
|
|
analogWrite(PWM4, 0);
|
|
pinMode(latchPin0, OUTPUT);
|
|
pinMode(clockPin0, OUTPUT);
|
|
pinMode(dataPin0, OUTPUT);
|
|
resetshift0();
|
|
pinMode(latchPin1, OUTPUT);
|
|
pinMode(clockPin1, OUTPUT);
|
|
pinMode(dataPin1, OUTPUT);
|
|
resetshift1();
|
|
ini();
|
|
|
|
}
|
|
|
|
//0, 1 aletas delanteras arriba y abajo
|
|
//2, 3 aletas delanteras GIRO
|
|
boolean reposo = true;
|
|
|
|
void reset_aletas()
|
|
{
|
|
aleta_izquierda(-255, 35000);
|
|
aleta_derecha(-255, 35000);
|
|
cola(-255, 35000);
|
|
control();
|
|
//aleta_derecha_giro(-255, 35000);
|
|
//aleta_izquierda_giro(-255, 35000);
|
|
control();
|
|
//aleta_derecha_giro(255, 4500);
|
|
//aleta_izquierda_giro(255, 4200);
|
|
control();
|
|
}
|
|
|
|
void ini()
|
|
{
|
|
reset_costillas();
|
|
//giro_cuerpo(UP, 120000);
|
|
control();
|
|
reset_aletas();
|
|
}
|
|
|
|
int SPEED = 255;
|
|
int RAMP = 25;
|
|
|
|
void ballena()
|
|
{
|
|
for (int i=0; i<1; i++)
|
|
{
|
|
for (int j=0; j<RAMP; j++)
|
|
{
|
|
aleta_izquierda(j*SPEED/RAMP, 100);
|
|
aleta_derecha(j*SPEED/RAMP, 100);
|
|
//aleta_derecha_giro(j*SPEED/RAMP, 22);
|
|
//aleta_izquierda_giro(j*SPEED/RAMP, 22);
|
|
cola(-j*SPEED/RAMP, 100);
|
|
for (int h = 0; h < 10; h=h+2)
|
|
{
|
|
costilla(h, SPEED/3, 10);
|
|
costilla(h+1, SPEED/3, 10);
|
|
}
|
|
control();
|
|
}
|
|
aleta_izquierda(255, 4000);
|
|
aleta_derecha(255, 4000);
|
|
cola(-255, 6000);
|
|
control();
|
|
for (int j=0; j<RAMP; j++)
|
|
{
|
|
aleta_izquierda(-j*SPEED/RAMP, 100);
|
|
aleta_derecha(-j*SPEED/RAMP, 100);
|
|
//aleta_derecha_giro(-j*SPEED/RAMP, 20);
|
|
//aleta_izquierda_giro(-j*SPEED/RAMP, 20);
|
|
cola(j*SPEED/RAMP, 100);
|
|
for (int h = 0; h < 10; h=h+2)
|
|
{
|
|
costilla(h, -SPEED/3, 20);
|
|
costilla(h+1, -SPEED/3, 20);
|
|
}
|
|
control();
|
|
}
|
|
aleta_izquierda(-255, 4000);
|
|
aleta_derecha(-255, 4000);
|
|
cola(255, 6000);
|
|
control();
|
|
}
|
|
// for (int i=0; i<2; i++)
|
|
// {
|
|
// for (int j=0; j<RAMP; j++)
|
|
// {
|
|
// aleta_izquierda(j*SPEED/RAMP, 100);
|
|
// aleta_derecha(j*SPEED/RAMP, 100);
|
|
// aleta_derecha_giro(j*SPEED/RAMP, 60);
|
|
// aleta_izquierda_giro(j*SPEED/RAMP, 60);
|
|
// cola(-j*SPEED/RAMP, 100);
|
|
// for (int h = 0; h < 10; h=h+2)
|
|
// {
|
|
// costilla(h, SPEED/3, 10);
|
|
// costilla(h+1, SPEED/3, 10);
|
|
// }
|
|
// control();
|
|
// }
|
|
// aleta_izquierda(255, 8000);
|
|
// aleta_derecha(255, 8000);
|
|
// cola(-255, 6000);
|
|
// control();
|
|
// for (int j=0; j<RAMP; j++)
|
|
// {
|
|
// aleta_izquierda(-j*SPEED/RAMP, 100);
|
|
// aleta_derecha(-j*SPEED/RAMP, 100);
|
|
// aleta_derecha_giro(-j*SPEED/RAMP, 20);
|
|
// aleta_izquierda_giro(-j*SPEED/RAMP, 20);
|
|
// cola(j*SPEED/RAMP, 100);
|
|
// for (int h = 0; h < 10; h=h+2)
|
|
// {
|
|
// costilla(h, -SPEED/3, 30);
|
|
// costilla(h+1, -SPEED/3, 30);
|
|
// }
|
|
// control();
|
|
// }
|
|
// aleta_izquierda(-255, 8000);
|
|
// aleta_derecha(-255, 8000);
|
|
// cola(255, 6000);
|
|
// control();
|
|
// }
|
|
// for (int i=0; i<4; i++)
|
|
// {
|
|
// for (int j=0; j<RAMP; j++)
|
|
// {
|
|
// aleta_izquierda(j*SPEED/RAMP, 100);
|
|
// aleta_derecha(j*SPEED/RAMP, 100);
|
|
// aleta_derecha_giro(j*SPEED/RAMP, 80);
|
|
// aleta_izquierda_giro(j*SPEED/RAMP, 80);
|
|
// cola(-j*SPEED/RAMP, 100);
|
|
// for (int h = 0; h < 10; h=h+2)
|
|
// {
|
|
// costilla(h, SPEED/3, 10);
|
|
// costilla(h+1, SPEED/3, 10);
|
|
// }
|
|
// control();
|
|
// }
|
|
// aleta_izquierda(255, 4000);
|
|
// aleta_derecha(255, 4000);
|
|
// cola(-255, 6000);
|
|
// control();
|
|
// aleta_izquierda(0, 1000);
|
|
// aleta_derecha(0, 1000);
|
|
// control();
|
|
// for (int j=0; j<RAMP; j++)
|
|
// {
|
|
// aleta_izquierda(-j*SPEED/RAMP, 100);
|
|
// aleta_derecha(-j*SPEED/RAMP, 100);
|
|
// aleta_derecha_giro(-j*SPEED/RAMP, 20);
|
|
// aleta_izquierda_giro(-j*SPEED/RAMP, 20);
|
|
// cola(j*SPEED/RAMP, 100);
|
|
// for (int h = 0; h < 10; h=h+2)
|
|
// {
|
|
// costilla(h, -SPEED/3, 40);
|
|
// costilla(h+1, -SPEED/3, 40);
|
|
// }
|
|
// control();
|
|
// }
|
|
// aleta_izquierda(-255, 4000);
|
|
// aleta_derecha(-255, 4000);
|
|
// cola(255, 6000);
|
|
// control();
|
|
// }
|
|
// for (int i=0; i<1; i++)
|
|
// {
|
|
// for (int j=0; j<RAMP; j++)
|
|
// {
|
|
// aleta_izquierda(j*SPEED/RAMP, 100);
|
|
// aleta_derecha(j*SPEED/RAMP, 100);
|
|
// aleta_derecha_giro(j*SPEED/RAMP, 100);
|
|
// aleta_izquierda_giro(j*SPEED/RAMP, 100);
|
|
// cola(-j*SPEED/RAMP, 100);
|
|
// for (int h = 0; h < 10; h=h+2)
|
|
// {
|
|
// costilla(h, SPEED/3, 10);
|
|
// costilla(h+1, SPEED/3, 10);
|
|
// }
|
|
// control();
|
|
// }
|
|
// aleta_izquierda(255, 8000);
|
|
// aleta_derecha(255, 8000);
|
|
// cola(-255, 6000);
|
|
// control();
|
|
// for (int j=0; j<RAMP; j++)
|
|
// {
|
|
// aleta_izquierda(-j*SPEED/RAMP, 100);
|
|
// aleta_derecha(-j*SPEED/RAMP, 100);
|
|
// aleta_derecha_giro(-j*SPEED/RAMP, 20);
|
|
// aleta_izquierda_giro(-j*SPEED/RAMP, 20);
|
|
// cola(j*SPEED/RAMP, 100);
|
|
// for (int h = 0; h < 10; h=h+2)
|
|
// {
|
|
// costilla(h, -SPEED/3, 50);
|
|
// costilla(h+1, -SPEED/3, 50);
|
|
// }
|
|
// control();
|
|
// }
|
|
// aleta_izquierda(-255, 8000);
|
|
// aleta_derecha(-255, 8000);
|
|
// cola(255, 6000);
|
|
// control();
|
|
// }
|
|
//
|
|
}
|
|
|
|
void barco()
|
|
{
|
|
aleta_izquierda(255, 35000);
|
|
aleta_derecha(255, 35000);
|
|
cola(255, 35000);
|
|
control();
|
|
aleta_derecha_giro(255, 15000);
|
|
aleta_izquierda_giro(255, 15000);
|
|
control();
|
|
giro_cuerpo(DOWN, 120000);
|
|
control();
|
|
for (int i = 0; i < 10; i++) costilla(i, 255, 10000);
|
|
control();
|
|
if (reposo)
|
|
{
|
|
time_sensor=millis();
|
|
while((millis()-time_sensor)<=TIME_STOP)
|
|
{
|
|
if (!fcread(12))
|
|
{
|
|
for (int i = 0; i < 10; i=i+2)
|
|
{
|
|
costilla(i, -SPEED, 2000);
|
|
costilla(i+1, -SPEED, 2000);
|
|
if (i==0) cola(-255,500);
|
|
if ((i==0)||(i==4)||(i==8))
|
|
{
|
|
aleta_derecha_giro(-255, 500);
|
|
aleta_izquierda_giro(-255, 500);
|
|
}
|
|
else
|
|
{
|
|
aleta_derecha_giro(255, 500);
|
|
aleta_izquierda_giro(255, 500);
|
|
}
|
|
control();
|
|
}
|
|
for (int i = 0; i < 10; i=i+2)
|
|
{
|
|
costilla(i, SPEED, 2000);
|
|
costilla(i+1, SPEED, 2000);
|
|
if (i==0) cola(255,500);
|
|
if ((i==2)||(i==6)||(i==10))
|
|
{
|
|
aleta_derecha_giro(-255, 500);
|
|
aleta_izquierda_giro(-255, 500);
|
|
}
|
|
else
|
|
{
|
|
aleta_derecha_giro(255, 500);
|
|
aleta_izquierda_giro(255, 500);
|
|
}
|
|
control();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
aleta_derecha_giro(255, 500);
|
|
aleta_izquierda_giro(255, 500);
|
|
control();
|
|
aleta_derecha_giro(0, 500);
|
|
aleta_izquierda_giro(0, 500);
|
|
control();
|
|
aleta_derecha_giro(-255, 500);
|
|
aleta_izquierda_giro(-255, 500);
|
|
control();
|
|
aleta_derecha_giro(0, 500);
|
|
aleta_izquierda_giro(0, 500);
|
|
control();
|
|
}
|
|
aleta_derecha_giro(255, 500);
|
|
aleta_izquierda_giro(255, 500);
|
|
control();
|
|
|
|
// for (int j=0; j<10; j++)
|
|
// {
|
|
// for (int i = 0; i < 10; i=i+2)
|
|
// {
|
|
// costilla(i, -SPEED, 2000);
|
|
// costilla(i+1, -SPEED, 2000);
|
|
// if (i==0) cola(-255,500);
|
|
// if ((i==0)||(i==4)||(i==8))
|
|
// {
|
|
// aleta_derecha_giro(-255, 500);
|
|
// aleta_izquierda_giro(-255, 500);
|
|
// }
|
|
// else
|
|
// {
|
|
// aleta_derecha_giro(255, 500);
|
|
// aleta_izquierda_giro(255, 500);
|
|
// }
|
|
// control();
|
|
// }
|
|
// for (int i = 0; i < 10; i=i+2)
|
|
// {
|
|
// costilla(i, SPEED, 2000);
|
|
// costilla(i+1, SPEED, 2000);
|
|
// if (i==0) cola(255,500);
|
|
// if ((i==2)||(i==6)||(i==10))
|
|
// {
|
|
// aleta_derecha_giro(-255, 500);
|
|
// aleta_izquierda_giro(-255, 500);
|
|
// }
|
|
// else
|
|
// {
|
|
// aleta_derecha_giro(255, 500);
|
|
// aleta_izquierda_giro(255, 500);
|
|
// }
|
|
// control();
|
|
// }
|
|
// }
|
|
ini();
|
|
}
|
|
|
|
|
|
void loop() {
|
|
//for(int i=0; i<40; i++) Serial.print(fcread(i));
|
|
// Serial.println(fcread(12));
|
|
//ballena();
|
|
if (reposo)
|
|
{
|
|
time_sensor=millis();
|
|
while((millis()-time_sensor)<=TIME_STOP)
|
|
{
|
|
if (!fcread(12))
|
|
{
|
|
ballena();
|
|
}
|
|
}
|
|
}
|
|
// barco();
|
|
ini();
|
|
}
|