Code:
/
#include <WProgram.h>
const int RMotor1 = 2; // H-bridge leg 3 Input 1 14,4V Motor
const int RMotor2 = 4; // H-bridge leg 2 Input 2
const int RSpeed = 3; // H-bridge leg 7 Enable A (PWM)
const int LMotor1 = 7; // H-bridge leg 13 Input 3 12V Motor
const int LMotor2 = 8; // H-bridge leg 14 Input 4
const int LSpeed = 6; // H-bridge leg 10 Enable B (PWM)
const int ledBlue = 12; // BlueLED left Motor
const int ledRed = 13; // RedLED right Motor
int PinOdoR = 1; // right Pin for IR-Sensor
int PinOdoL = 0; // left Pin for IR-Sensor
// int RSpeed = 120; // is not needed in this Kontex
// int LSpeed = 120;
// int mDirR = 0;
// int mDirL = 0;
// int LedPwmR = 0;
// int LedPwmB = 0;
int readAnalog(int port); // Why are these functions initialized here?
int readOdoR();
int readOdoL();
void blink(int whatPin, int howManyTimes, int milliSecs);
// void motorR (int InputPin2, int InputPin4, int pwm3);
// void motorL (int InputPin7, int InputPin8, int pwm6);
void motorStop();
//-------------------------- Setup ----------------------------------------------------
void setup() {
void motorStop();
pinMode(RMotor1, OUTPUT);
pinMode(RMotor2, OUTPUT);
pinMode(RSpeed, OUTPUT);
pinMode(LMotor1, OUTPUT);
pinMode(LMotor2, OUTPUT);
pinMode(LSpeed, OUTPUT);
pinMode(ledBlue, OUTPUT);
pinMode(ledRed, OUTPUT);
Serial.begin(115200);
// motorStop(); // would be necessary at all?
Serial.println("***********************");
Serial.println("* Encoder-Test *");
Serial.print("* ");Serial.print(__DATE__);Serial.print(" ");Serial.print(__TIME__);Serial.println("*");
Serial.println("***********************");
blink(ledBlue, 3, 1000);
}
//------------------------------------------------------------------------------
void loop() {
int RSpeed = 120;
int LSpeed = 120;
motorR(HIGH,LOW,RSpeed);
motorL(HIGH,LOW,LSpeed);
delay(2000);
Serial.println(" Encoder Rechts/Links ");
for (int i = 0; i <= 52; i++)
{
int valR = readOdoR();
int valL = readOdoL();
Serial.print(" ");Serial.print(valR,DEC);Serial.print(" ;");Serial.print(valL,DEC);Serial.println(" ");
delay (10); // hängt dieser Wert von der Drehzahl des Encoders ab?
if (valR > 400) { digitalWrite(ledRed,HIGH); }
else { digitalWrite(ledRed,LOW); }
if (valL > 400) { digitalWrite(ledBlue,HIGH); }
else { digitalWrite(ledBlue,LOW); }
}
Serial.println(" ");
// motorStop();
Serial.println("***********************");
delay (5000);
}
// -------------------------- Left Motor -----------------------------------------------
void motorR (int InputPin2, int InputPin4, int pwm3){
digitalWrite(RMotor1, InputPin2);
digitalWrite(RMotor2, InputPin4);
analogWrite(RSpeed, pwm3);
}
//-------------------------- Right Motor ----------------------------------------------
void motorL (int InputPin7, int InputPin8, int pwm6){
digitalWrite(LMotor1, InputPin7);
digitalWrite(LMotor2, InputPin8);
analogWrite(LSpeed, pwm6);
}
//-------------------------- Stop Motor ----------------------------------------------
void motorStop (){
motorL(LOW, LOW, 0);
motorR(LOW, LOW, 0);
}
//------------------------------------------------------------------------------
int readOdoR(){
return readAnalog (PinOdoR);
}
//------------------------------------------------------------------------------
int readOdoL(){
return readAnalog (PinOdoL);
}
//------------------------------------------------------------------------------
int readAnalog(int port){
int val = 0;
for (int i = 0; i <= 3; i++)
{ val = val + analogRead(port); }
val = val/3;
return val;
}
//-------------------------- blinks an LED ------------------------------------
void blink(int whatPin, int howManyTimes, int milliSecs) {
int i = 0;
for ( i = 0; i < howManyTimes; i++) {
digitalWrite(whatPin, HIGH);
delay(milliSecs/2);
digitalWrite(whatPin, LOW);
delay(milliSecs/2);
}
}
//------------------------- Programm Ende ---------------------------------------
int main(void) {
/* Must call init for arduino to work properly */
init();
setup();
for (;;) {
loop();
} // end for
} // end main
Ist bestimmt ein typische Anfängercode, aber in der Arduino IDE funktioniert er.
Lesezeichen