Scribd Logo
Upload

Welcome to Scribd!

  • Xe 12

    Uploaded by

    snphehe1
    2 views5 pages
    This document defines functions to control a servo motor and DC motors to move a robot. It uses an ultrasonic sensor to measure distance and a PID controller to maintain a target distance of 20 cm when moving forward or backward. The robot can move forward, backward, turn left or right by controlling the direction of the two DC motors.

    Original Description:

    Original Title

    xe12

    Copyright

    © © All Rights Reserved

    Available Formats

    DOCX, PDF, TXT or read online from Scribd

    Did you find this document useful?

    Is this content inappropriate?

    Report this Document
    This document defines functions to control a servo motor and DC motors to move a robot. It uses an ultrasonic sensor to measure distance and a PID controller to maintain a target distance of 20 cm when moving forward or backward. The robot can move forward, backward, turn left or right by controlling the direction of the two DC motors.

    Copyright:

    © All Rights Reserved
    Download as DOCX, PDF, TXT or read online from Scribd
    Flag for inappropriate content
    Download as docx, pdf, or txt
    2 views5 pages

    Xe 12

    Uploaded by

    snphehe1
    This document defines functions to control a servo motor and DC motors to move a robot. It uses an ultrasonic sensor to measure distance and a PID controller to maintain a target distance of 20 cm when moving forward or backward. The robot can move forward, backward, turn left or right by controlling the direction of the two DC motors.

    Copyright:

    © All Rights Reserved
    Download as DOCX, PDF, TXT or read online from Scribd
    Flag for inappropriate content
    Download as docx, pdf, or txt
    of 5

    #include <Servo.

    h>

    #include <PID_v1.h>

    Servo myservo;

    int pos = 0;

    const int trig = 7;

    const int echo = 4;

    int enbA = 3;

    int in1 = 5;

    int in2 = 6;

    int in3 = 9;

    int in4 = 10;

    int enbB = 11;

    int dongcoservo = 8;

    int gioihan = 20;

    int khoangcach;

    double Setpoint, Input, Output;

    double Kp = 2, Ki = 5, Kd = 1;

    PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

    void dokhoangcach();

    void dithang();

    void dunglai();

    void disangphai();

    void disangtrai();

    void dilui();

    void resetdongco();

    void quaycbsangphai();

    void quaycbsangtrai();
    void setup() {

    myservo.attach(dongcoservo);

    pinMode(trig, OUTPUT);

    pinMode(echo, INPUT);

    pinMode(in1, OUTPUT);

    pinMode(in2, OUTPUT);

    pinMode(in3, OUTPUT);

    pinMode(in4, OUTPUT);

    pinMode(enbA, OUTPUT);

    pinMode(enbB, OUTPUT);

    digitalWrite(in1, LOW);

    digitalWrite(in2, LOW);

    digitalWrite(in3, LOW);

    digitalWrite(in4, LOW);

    digitalWrite(enbA, LOW);

    digitalWrite(enbB, LOW);

    myservo.write(90);

    delay(500);

    Serial.begin(9600);

    Setpoint = 20;

    myPID.SetMode(AUTOMATIC);

    myPID.SetOutputLimits(-255, 255);

    myPID.SetSampleTime(100);

    }
    void loop() {

    khoangcach = 0;

    dokhoangcach();

    Serial.println(khoangcach);

    Input = khoangcach;

    myPID.Compute();

    if (Output > 0) {

    analogWrite(enbA, Output);

    analogWrite(enbB, 0);

    dithang();

    } else if (Output < 0) {

    analogWrite(enbA, 0);

    analogWrite(enbB, abs(Output));

    dilui();

    } else {

    dunglai();

    void dithang() {

    digitalWrite(in1, 0);

    digitalWrite(in2, 1);

    digitalWrite(in3, 0);

    digitalWrite(in4, 1);

    void dunglai() {

    analogWrite(enbA, 0);
    analogWrite(enbB, 0);

    digitalWrite(in1, 0);

    digitalWrite(in2, 0);

    digitalWrite(in3, 0);

    digitalWrite(in4, 0);

    void disangphai() {

    digitalWrite(in1, 0);

    digitalWrite(in2, 1);

    digitalWrite(in3, 0);

    digitalWrite(in4, 0);

    void disangtrai() {

    digitalWrite(in1, 0);

    digitalWrite(in2, 0);

    digitalWrite(in3, 0);

    digitalWrite(in4, 1);

    void dilui() {

    digitalWrite(in1, 1);

    digitalWrite(in2, 0);

    digitalWrite(in3, 1);

    digitalWrite(in4, 0);

    void dokhoangcach() {

    digitalWrite(trig, LOW);

    delayMicroseconds(2);
    digitalWrite(trig, HIGH);

    delayMicroseconds(10);

    digitalWrite(trig, LOW);

    unsigned long thoigian = pulseIn(echo, HIGH);

    khoangcach = thoigian / 2 / 29.412;

    void quaycbsangtrai() {

    myservo.write(150);

    delay(500);

    dokhoangcach();

    myservo.write(90);

    void quaycbsangphai() {

    myservo.write(30);

    delay(500);

    dokhoangcach();

    myservo.write(90);

    You might also like