Code:
// DIR und STEP pins definieren
#define dirPin_VL 6 //2
#define stepPin_VL 7 //3
#define dirPin_HL 8 //4
#define stepPin_HL 9 //5
#define dirPin_VR 2 //6
#define stepPin_VR 3 //7
#define dirPin_HR 4 //8
#define stepPin_HR 5 //9
//enable pins definieren
#define enbl_VL 40
#define enbl_HL 42
#define enbl_VR 41
#define enbl_HR 43
//resett pin definieren
#define PIN2RESET 10
//steps pro umdrehung definieren:
#define stepsPerRevolution 200
uint8_t taste_neu = 1;
uint8_t taste_alt = 8;
uint8_t taste = 0;
void setup()
{
// pinMode(LED, OUTPUT);
//pins als output:
pinMode(dirPin_VL, OUTPUT);
pinMode(stepPin_VL, OUTPUT);
pinMode(enbl_VL, OUTPUT);
pinMode(dirPin_HL, OUTPUT);
pinMode(stepPin_HL, OUTPUT);
pinMode(enbl_HL, OUTPUT);
pinMode(dirPin_VR, OUTPUT);
pinMode(stepPin_VR, OUTPUT);
pinMode(enbl_VR, OUTPUT);
pinMode(dirPin_HR, OUTPUT);
pinMode(stepPin_HR, OUTPUT);
pinMode(enbl_HR, OUTPUT);
//resett pin zustand definieren
pinMode(PIN2RESET, INPUT);
Serial.begin(115200);
Serial1.begin(115200);
Serial.println("code----- /home/georg/Arduino/outdoor_robo/smartphone_steuerung/outdoor_FB_switch_bluetooth_1.ino");
Serial.println("bluetooth übertragung!");
//enable pins deaktivieren:
digitalWrite(enbl_VL, LOW);
digitalWrite(enbl_HL, LOW);
digitalWrite(enbl_VR, LOW);
digitalWrite(enbl_HR, LOW);
//resett pin aktivieren
digitalWrite(PIN2RESET, HIGH);
}
void loop()
{
while (Serial1.available())
{
taste = Serial1.read();
Serial.println(taste);
// taste_neu = taste;
tasten_abfrage();
}
}
/***********************************************************/
void tasten_abfrage(void)
{
switch (taste)
{
case 100:// rotate rechts - FB quer smartphone
{
rechts_drehen();
break;
}
case 97:// rotate links - FB quer smartphone
{
links_drehen();
break;
}
case 116:// fahre vor - FB quer smartphone
{
vorwaerts();
break;
}
case 115:// fahre rückwärts - FB quer smartphone
{
rueckwaerts();
break;
}
}
}
/***********************************************************/
void alle_stepper_stop(void)
{
//enable pins deaktivieren
digitalWrite(enbl_VL, LOW);
digitalWrite(enbl_HL, LOW);
digitalWrite(enbl_VR, LOW);
digitalWrite(enbl_HR, LOW);
reboot();
}
/***********************************************************/
void vorwaerts(void)
{
// enable pins aktivieren:
digitalWrite(enbl_VL, HIGH);
digitalWrite(enbl_HL, HIGH);
digitalWrite(enbl_VR, HIGH);
digitalWrite(enbl_HR, HIGH);
//richtung bestimmen
digitalWrite(dirPin_VL, LOW);
digitalWrite(dirPin_HL, LOW);
digitalWrite(dirPin_VR, HIGH);
digitalWrite(dirPin_HR, HIGH);
for (int i = 0; i < stepsPerRevolution; i++)
{
digitalWrite(stepPin_VL, HIGH);
digitalWrite(stepPin_HL, HIGH);
digitalWrite(stepPin_VR, HIGH);
digitalWrite(stepPin_HR, HIGH);
delayMicroseconds(500);
digitalWrite(stepPin_VL, LOW);
digitalWrite(stepPin_HL, LOW);
digitalWrite(stepPin_VR, LOW);
digitalWrite(stepPin_HR, LOW);
delayMicroseconds(500);
}
tasten_abfrage();
}
/**********************************************************/
void rueckwaerts(void)
{
// enable pins aktivieren:
digitalWrite(enbl_VL, HIGH);
digitalWrite(enbl_HL, HIGH);
digitalWrite(enbl_VR, HIGH);
digitalWrite(enbl_HR, HIGH);
//richtung bestimmen
digitalWrite(dirPin_VL, HIGH);
digitalWrite(dirPin_HL, HIGH);
digitalWrite(dirPin_VR, LOW);
digitalWrite(dirPin_HR, LOW);
for (int i = 0; i < stepsPerRevolution; i++)
{
digitalWrite(stepPin_VL, HIGH);
digitalWrite(stepPin_HL, HIGH);
digitalWrite(stepPin_VR, HIGH);
digitalWrite(stepPin_HR, HIGH);
delayMicroseconds(500);
digitalWrite(stepPin_VL, LOW);
digitalWrite(stepPin_HL, LOW);
digitalWrite(stepPin_VR, LOW);
digitalWrite(stepPin_HR, LOW);
delayMicroseconds(500);
}
tasten_abfrage();
}
/***********************************************************/
void rechts_drehen(void)
{
// enable pins aktivieren
digitalWrite(enbl_VL, HIGH);
digitalWrite(enbl_HL, HIGH);
digitalWrite(enbl_VR, HIGH);
digitalWrite(enbl_HR, HIGH);
//richtung bestimmen
digitalWrite(dirPin_VL, LOW);
digitalWrite(dirPin_HL, LOW);
digitalWrite(dirPin_VR, LOW);
digitalWrite(dirPin_HR, LOW);
for (int i = 0; i < stepsPerRevolution; i++)
{
digitalWrite(stepPin_VL, HIGH);
digitalWrite(stepPin_HL, HIGH);
digitalWrite(stepPin_VR, HIGH);
digitalWrite(stepPin_HR, HIGH);
delayMicroseconds(500);
digitalWrite(stepPin_VL, LOW);
digitalWrite(stepPin_HL, LOW);
digitalWrite(stepPin_VR, LOW);
digitalWrite(stepPin_HR, LOW);
delayMicroseconds(500);
}
tasten_abfrage();
}
/**********************************************************/
void links_drehen(void)
{
//enable pins aktivieren
digitalWrite(enbl_VL, HIGH);
digitalWrite(enbl_HL, HIGH);
digitalWrite(enbl_VR, HIGH);
digitalWrite(enbl_HR, HIGH);
//richtung bestimmen
digitalWrite(dirPin_VL, HIGH);
digitalWrite(dirPin_HL, HIGH);
digitalWrite(dirPin_VR, HIGH);
digitalWrite(dirPin_HR, HIGH);
for (int i = 0; i < stepsPerRevolution; i++)
{
digitalWrite(stepPin_VL, HIGH);
digitalWrite(stepPin_HL, HIGH);
digitalWrite(stepPin_VR, HIGH);
digitalWrite(stepPin_HR, HIGH);
delayMicroseconds(500);
digitalWrite(stepPin_VL, LOW);
digitalWrite(stepPin_HL, LOW);
digitalWrite(stepPin_VR, LOW);
digitalWrite(stepPin_HR, LOW);
delayMicroseconds(500);
}
tasten_abfrage();
}
/*
void links_drehen(void) //mit beschleunigung und sound
{
//enable pins aktivieren
digitalWrite(enbl_VL, HIGH);
digitalWrite(enbl_HL, HIGH);
digitalWrite(enbl_VR, HIGH);
digitalWrite(enbl_HR, HIGH);
//richtung bestimmen
digitalWrite(dirPin_VL, HIGH);
digitalWrite(dirPin_HL, HIGH);
digitalWrite(dirPin_VR, HIGH);
digitalWrite(dirPin_HR, HIGH);
//"j = 2000": der Anfangswert für das Delay in Microsekunden
for (int i = 0, j = 2000; i < stepsPerRevolution; i++)
{
//"j > 500": der Endwert für das Delay in Microsekunden - bestimmt auch die Endgeschwindigkeit (mindestens 250 bis 300)
if (j > 300) j--;
//"z < 3": wieviele Schritte mit dem eingstellten Delay "j" gemacht werden sollen, bevor es reduziert wird
for (int z = 0; z < 3 && i < stepsPerRevolution; z++)
{
digitalWrite(stepPin_VL, HIGH);
digitalWrite(stepPin_HL, HIGH);
digitalWrite(stepPin_VR, HIGH);
digitalWrite(stepPin_HR, HIGH);
delayMicroseconds(j);
digitalWrite(stepPin_VL, LOW);
digitalWrite(stepPin_HL, LOW);
digitalWrite(stepPin_VR, LOW);
digitalWrite(stepPin_HR, LOW);
delayMicroseconds(j);
i++;
}
}
}
*/
/***********************************************************/
void reboot()
{
pinMode(PIN2RESET, OUTPUT);
digitalWrite(PIN2RESET, LOW);
delay(100);
}
/***********************************************************/
/************************************************************/
Lesezeichen