350 lines
8.3 KiB
Arduino
350 lines
8.3 KiB
Arduino
|
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[6] = { 0, 0, 0, 0, 0, 0 };
|
||
|
|
unsigned long time_final_aleta[6] = { 0, 0, 0, 0, 0, 0 };
|
||
|
|
unsigned long time_giro = 0;
|
||
|
|
unsigned long time_final_giro = 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 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 0:
|
||
|
|
digitalWrite(A, HIGH);
|
||
|
|
digitalWrite(B, HIGH);
|
||
|
|
digitalWrite(C, LOW);
|
||
|
|
break;
|
||
|
|
case 1:
|
||
|
|
digitalWrite(A, LOW);
|
||
|
|
digitalWrite(B, LOW);
|
||
|
|
digitalWrite(C, LOW);
|
||
|
|
break;
|
||
|
|
case 2:
|
||
|
|
digitalWrite(A, HIGH);
|
||
|
|
digitalWrite(B, LOW);
|
||
|
|
digitalWrite(C, LOW);
|
||
|
|
break;
|
||
|
|
case 3:
|
||
|
|
digitalWrite(A, LOW);
|
||
|
|
digitalWrite(B, HIGH);
|
||
|
|
digitalWrite(C, LOW);
|
||
|
|
break;
|
||
|
|
case 4:
|
||
|
|
digitalWrite(A, LOW);
|
||
|
|
digitalWrite(B, LOW);
|
||
|
|
digitalWrite(C, HIGH);
|
||
|
|
break;
|
||
|
|
case 5:
|
||
|
|
digitalWrite(A, LOW);
|
||
|
|
digitalWrite(B, HIGH);
|
||
|
|
digitalWrite(C, HIGH);
|
||
|
|
break;
|
||
|
|
case 6:
|
||
|
|
digitalWrite(A, HIGH);
|
||
|
|
digitalWrite(B, HIGH);
|
||
|
|
digitalWrite(C, HIGH);
|
||
|
|
break;
|
||
|
|
case 7:
|
||
|
|
digitalWrite(A, HIGH);
|
||
|
|
digitalWrite(B, LOW);
|
||
|
|
digitalWrite(C, HIGH);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
boolean fcread(int pin)
|
||
|
|
{
|
||
|
|
int count = 0;
|
||
|
|
for (int j = 0; j < 5; j++)
|
||
|
|
{
|
||
|
|
for (int i = 0; i < 8; i++)
|
||
|
|
{
|
||
|
|
selgrup(i);
|
||
|
|
delay(2);
|
||
|
|
fc[count] = digitalRead(G[j]);
|
||
|
|
count++;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return fc[pin];
|
||
|
|
}
|
||
|
|
|
||
|
|
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)
|
||
|
|
{
|
||
|
|
if (num < 10)
|
||
|
|
{
|
||
|
|
if (value > 0) flag_dir_c[num] = 1;
|
||
|
|
else if (value < 0) flag_dir_c[num] = -1;
|
||
|
|
else flag_dir_c[num] = 0;
|
||
|
|
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_a[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 };
|
||
|
|
int fc_aleta_up[6] = {2, 0, 4, 7, 8, 10};
|
||
|
|
int fc_aleta_down[6]= {3, 1, 5, 6, 9, 11};
|
||
|
|
|
||
|
|
void aleta(int num, int value)
|
||
|
|
{
|
||
|
|
if (num < 6)
|
||
|
|
{
|
||
|
|
if (value > 0) flag_dir_a[num] = 1;
|
||
|
|
else if (value < 0) flag_dir_a[num] = -1;
|
||
|
|
else flag_dir_a[num] = 0;
|
||
|
|
|
||
|
|
if (value != 0)
|
||
|
|
{
|
||
|
|
shift1Write(en_a[num], true);
|
||
|
|
analogWrite(PWM[num], abs(value));
|
||
|
|
}
|
||
|
|
else shift1Write(en_a[num], false);
|
||
|
|
|
||
|
|
if ((value > 0)&&(digitalRead(fc_aleta_up[num]))) {
|
||
|
|
shift1Write(dirA[num], true);
|
||
|
|
shift1Write(dirB[num], false);
|
||
|
|
}
|
||
|
|
else if ((value < 0)&&(digitalRead(fc_aleta_down[num]))) {
|
||
|
|
shift1Write(dirA[num], false);
|
||
|
|
shift1Write(dirB[num], true);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
int flag_dir_g = 0;
|
||
|
|
|
||
|
|
void giro(int mode)
|
||
|
|
{
|
||
|
|
if (mode > 0)
|
||
|
|
{
|
||
|
|
flag_dir_g = 1;
|
||
|
|
digitalWrite(UP, HIGH);
|
||
|
|
digitalWrite(DOWN, LOW);
|
||
|
|
}
|
||
|
|
else if (mode < 0)
|
||
|
|
{
|
||
|
|
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;
|
||
|
|
|
||
|
|
|
||
|
|
void control()
|
||
|
|
{
|
||
|
|
for (int num = 0; num < 5; num++)
|
||
|
|
{
|
||
|
|
if ((flag_dir_a[num]==-1)&&(!digitalRead(fc_aleta_down[num]))) aleta(num,0);
|
||
|
|
else if ((flag_dir_a[num]==1)&&(!digitalRead(fc_aleta_up[num]))) aleta(num,0);
|
||
|
|
}
|
||
|
|
// 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)))
|
||
|
|
// aleta(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_giro = 1;
|
||
|
|
// if (flag_dir_g != 0)
|
||
|
|
// {
|
||
|
|
// giro(STOP,0);
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
}
|
||
|
|
void reset_costillas()
|
||
|
|
{
|
||
|
|
for (int i = 0; i < 10; i++) costilla(i, -255);
|
||
|
|
delay(10000);
|
||
|
|
for (int i = 0; i < 10; i++) costilla(i, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
Serial.begin(9600);
|
||
|
|
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();
|
||
|
|
reset_costillas();
|
||
|
|
reset_aletas();
|
||
|
|
}
|
||
|
|
|
||
|
|
void reset_aletas()
|
||
|
|
{
|
||
|
|
aleta(0, -255);
|
||
|
|
aleta(1, -255);
|
||
|
|
aleta(4, -255);
|
||
|
|
control();
|
||
|
|
aleta(3, -255);
|
||
|
|
control();
|
||
|
|
aleta(3, 255);
|
||
|
|
delay(4000);
|
||
|
|
aleta(3, 0);
|
||
|
|
giro(255);
|
||
|
|
delay(120000);
|
||
|
|
}
|
||
|
|
|
||
|
|
byte command = 0x00;
|
||
|
|
int speed_motor = 0x00;
|
||
|
|
int address = 0x00;
|
||
|
|
void serialEvent() {
|
||
|
|
while (Serial.available()) {
|
||
|
|
command = Serial.read();
|
||
|
|
address = command>>4;
|
||
|
|
speed_motor = command&0x0F;
|
||
|
|
if (speed_motor>=8) speed_motor = map(speed_motor, 8, 15, 0, 255);//speed_motor - 8;
|
||
|
|
else speed_motor = map(speed_motor, 0, 7, 0, -255); //speed_motor * (-1);
|
||
|
|
if (address == 0) giro(speed_motor);
|
||
|
|
else if (address >= 5) aleta(address - 1, speed_motor);
|
||
|
|
else costilla(address - 6, speed_motor);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
int M = 3;
|
||
|
|
int SPEED = 255;
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
control();
|
||
|
|
}
|