Der letzte Firmware
Hochgeladene Dateien:#define BRAKE 0
#define CW 1
#define CCW 2
#define CS_THRESHOLD 15 // Definition of safety current (Check: "1.3 Monster Shield Example").
//MOTOR 1
#define MOTOR_A1_PIN 7
#define MOTOR_B1_PIN 8
//MOTOR 2
#define MOTOR_A2_PIN 4
#define MOTOR_B2_PIN 9
#define PWM_MOTOR_1 5
#define PWM_MOTOR_2 6
#define CURRENT_SEN_1 A2
#define CURRENT_SEN_2 A3
#define EN_PIN_1 A0
#define EN_PIN_2 A1
#define MOTOR_1 0
#define MOTOR_2 1
#define usTrig 10
#define usEcho 11
bool isAuto = 0;
short leftSpeed = 255; //default motor speed
//unsigned short leftMotor_Status = BRAKE;
short rightSpeed = 255*0.5; //default motor speed
unsigned short usMotor_Status = BRAKE;
void setup()
{
pinMode(usTrig,OUTPUT);
pinMode(usEcho,INPUT);
pinMode(MOTOR_A1_PIN, OUTPUT);
pinMode(MOTOR_B1_PIN, OUTPUT);
pinMode(MOTOR_A2_PIN, OUTPUT);
pinMode(MOTOR_B2_PIN, OUTPUT);
pinMode(PWM_MOTOR_1, OUTPUT);
pinMode(PWM_MOTOR_2, OUTPUT);
pinMode(CURRENT_SEN_1, OUTPUT);
pinMode(CURRENT_SEN_2, OUTPUT);
pinMode(EN_PIN_1, OUTPUT);
pinMode(EN_PIN_2, OUTPUT);
Serial1.begin(9600); // Initiates the Serial1 to do the monitoring
Serial1.println("Begin motor control");
Serial1.println(); //Print function list for user selection
Serial1.println("Enter number for control option:");
Serial1.println("1. STOP");
Serial1.println("2. FORWARD");
Serial1.println("3. REVERSE");
Serial1.println("4. LEFT(FWD)");
Serial1.println("5. RIGHT(FWD)");
Serial1.println("6. LEFT(REV)");
Serial1.println("7. RIGHT(REV)");
Serial1.println("+. INCREASE SPEED");
Serial1.println("-. DECREASE SPEED");
Serial1.println("I. AUTOPILOT");
Serial1.println();
}
void loop()
{
char user_input;
while(Serial1.available())
{
user_input = Serial1.read(); //Read user input and trigger appropriate function
digitalWrite(EN_PIN_1, HIGH);
digitalWrite(EN_PIN_2, HIGH);
if( isAuto == 1 ){
automatic();
}
else{
if (user_input =='1')
{
Stop();
}
else if(user_input =='2')
{
Forward();
}
else if(user_input =='3')
{
Reverse();
}
else if(user_input =='4')
{
LeftForward();
}
else if(user_input =='5')
{
RightForward();
}
else if(user_input =='6')
{
LeftReverse();
}
else if(user_input =='7')
{
RightReverse();
}
else if(user_input =='+')
{
IncreaseSpeed();
}
else if(user_input =='-')
{
DecreaseSpeed();
}
else if(user_input =='I' || user_input == 'i')
{
isAuto = 1;
}
else
{
Serial1.println("Invalid option entered.");
}
}
}
}
void Stop()
{
Serial1.println("Stop");
usMotor_Status = BRAKE;
motorGo(MOTOR_1, usMotor_Status, 0);
motorGo(MOTOR_2, usMotor_Status, 0);
}
void Forward()
{
Serial1.println("Forward");
usMotor_Status = CW;
motorGo(MOTOR_1, usMotor_Status, leftSpeed);
motorGo(MOTOR_2, usMotor_Status, rightSpeed);
}
void Reverse()
{
Serial1.println("Reverse");
usMotor_Status = CCW;
motorGo(MOTOR_1, usMotor_Status, leftSpeed);
motorGo(MOTOR_2, usMotor_Status, rightSpeed);
}
void Left()
{
Serial1.println("Left");
usMotor_Status = CCW;
motorGo(MOTOR_1, CW, leftSpeed);
motorGo(MOTOR_2, usMotor_Status, rightSpeed);
}
void Right()
{
Serial1.println("Right");
usMotor_Status = CCW;
motorGo(MOTOR_1, usMotor_Status, leftSpeed);
motorGo(MOTOR_2, CW, rightSpeed);
}
void IncreaseSpeed()
{
leftSpeed = leftSpeed + 10;
if(leftSpeed > 255)
{
leftSpeed = 255;
}
Serial1.print("Speed +: ");
Serial1.println(leftSpeed);
rightSpeed = rightSpeed + 10;
if(rightSpeed > 255)
{
rightSpeed = 255;
}
Serial1.print("Speed +: ");
Serial1.println(rightSpeed);
motorGo(MOTOR_1, usMotor_Status, leftSpeed);
motorGo(MOTOR_2, usMotor_Status, rightSpeed);
}
void DecreaseSpeed()
{
leftSpeed = leftSpeed - 10;
if(leftSpeed < 0)
{
leftSpeed = 0;
}
Serial1.print("Speed -: ");
Serial1.println(leftSpeed);
rightSpeed = rightSpeed - 10;
if(rightSpeed < 0)
{
rightSpeed = 0;
}
Serial1.print("Speed -: ");
Serial1.println(rightSpeed);
motorGo(MOTOR_1, usMotor_Status, leftSpeed);
motorGo(MOTOR_2, usMotor_Status, rightSpeed);
}
void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm) //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e pwm (entra 0 e 255);
{
if(motor == MOTOR_1)
{
if(direct == CW)
{
digitalWrite(MOTOR_A1_PIN, LOW);
digitalWrite(MOTOR_B1_PIN, HIGH);
}
else if(direct == CCW)
{
digitalWrite(MOTOR_A1_PIN, HIGH);
digitalWrite(MOTOR_B1_PIN, LOW);
}
else
{
digitalWrite(MOTOR_A1_PIN, LOW);
digitalWrite(MOTOR_B1_PIN, LOW);
}
analogWrite(PWM_MOTOR_1, pwm);
}
else if(motor == MOTOR_2)
{
if(direct == CW)
{
digitalWrite(MOTOR_A2_PIN, LOW);
digitalWrite(MOTOR_B2_PIN, HIGH);
}
else if(direct == CCW)
{
digitalWrite(MOTOR_A2_PIN, HIGH);
digitalWrite(MOTOR_B2_PIN, LOW);
}
else
{
digitalWrite(MOTOR_A2_PIN, LOW);
digitalWrite(MOTOR_B2_PIN, LOW);
}
analogWrite(PWM_MOTOR_2, pwm);
}
}
float usRead()
{
float tInCM = 0.0;
int t = 0;
digitalWrite(usTrig,HIGH); //Trigger-Signal senden
delay(10); //Warten
digitalWrite(usTrig,LOW);
t = pulseIn(usEcho,RISING); //Impuls auslesen; Sollte der Impuls nicht innerhalb einer Sekunde (1000000µS) zurückgesendet werden, setze Code fort
tInCM = t / 2;
tInCM = tInCM * 0.03432;
return(tInCM);
/*if ( tInCM > 0 && tInCM < 100 ){
return(tInCM);
}
else{
return(0);
}*/
}
void LeftForward()
{
Serial1.println("LeftForward");
usMotor_Status = CW;
motorGo(MOTOR_1, usMotor_Status, leftSpeed*0.5);
motorGo(MOTOR_2,usMotor_Status, rightSpeed);
}
void RightForward()
{
Serial1.println("RightForward");
usMotor_Status = CW;
motorGo(MOTOR_1, usMotor_Status, leftSpeed);
motorGo(MOTOR_2,usMotor_Status, rightSpeed*0.5);
}
void LeftReverse()
{
Serial1.println("LeftReverse");
usMotor_Status = CCW;
motorGo(MOTOR_1, usMotor_Status, leftSpeed*0.5);
motorGo(MOTOR_2,usMotor_Status, rightSpeed);
}
void RightReverse()
{
Serial1.println("RightReverse");
usMotor_Status = CCW;
motorGo(MOTOR_1, usMotor_Status, leftSpeed);
motorGo(MOTOR_2,usMotor_Status, rightSpeed*0.5);
}
void automatic()
{
Forward();
while( isAuto == 1 ){
if( Serial1.available() ){
if( Serial1.read() == 'O' || Serial1.read() == 'o'){
Serial1.println("Automatic mode is turned off!");
Stop();
isAuto = 0;
}
}
if( usRead() < 50 && usRead() > 0 ){
Serial1.print("Object detected: ");
Serial1.println(usRead());
Stop();
LeftReverse();
delay(5000);
Stop();
Forward();
}
}
}
- Du musst dich anmelden um auf Uploads zugreifen zu können.