Maker Forum

Du musst dich anmelden um Beiträge und Themen zu erstellen.

Der letzte Firmware

#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();

}

}

}

 

Hochgeladene Dateien:
  • Du musst dich anmelden um auf Uploads zugreifen zu können.