只是一个新手,有一些错误及不完善的地方麻烦提一下。亲测是可以简单的实现的。把引脚这些定义到自己创的.h文件有助于整理。
制作出来的实物图如下:
Arduino源程序如下:
- include"arduino.h"
- #define inputPin A5 //超声波触发引脚
- #define outputPin A4 //超声波接收引脚
- int fm = A0;
- /*int K1;
- int K2;*/
- int limit_switch0 = A1;
- const byte right_motor_encoder_a = 2;
- const byte right_motor_encoder_b = 7;
- const byte left_motor_encoder_a = 3;
- const byte left_motor_encoder_b = 12;
- int Right_motor_go =9; //L298P直流电机驱动板的使能端口连接到数字接口8
- int Right_motor_back =10; //L298P直流电机驱动板的转向端口连接到数字接口9
- int Left_motor_go =5; //L298P直流电机驱动板的使能端口连接到数字接口5
- int Left_motor_back =6; //L298P直流电机驱动板的转向端口连接到数字接口4
- double right_position;//右轮脉冲数
- double right_position_pre;
- double left_position;//左轮脉冲数
- double left_position_pre;
- boolean right_Direction;//右轮旋转方向
- boolean left_Direction;//左轮旋转方向
- double speed_right;
- double speed_left;
- boolean result;
- unsigned long before = 0;
- unsigned long current = 0;
- unsigned long interval = 100;
复制代码- #include"zzz.h"
- #include "U8g2lib.h"
- #ifdef U8X8_HAVE_HW_SPI
- #include <SPI.h>
- #endif
- #ifdef U8X8_HAVE_HW_I2C
- #include <Wire.h>
- #endif
- U8G2_SSD1306_128X64_NONAME_1_4W_SW_SPI u8g2(U8G2_R0, /* clock=*/ 13, /* data=*/ 11, /* cs=*/ A3, /* dc=*/ 4, /* reset=*/ 8);
- void setup()
- {
- Serial.begin(9600);//Initialize the serial port
- u8g2.begin();
- pinMode(fm,OUTPUT);
- pinMode(inputPin,INPUT_PULLUP);
- pinMode(outputPin,OUTPUT);
- //初始化电机驱动IO为输出方式
- pinMode(Left_motor_go,OUTPUT);
- pinMode(Left_motor_back,OUTPUT);
- pinMode(Right_motor_go,OUTPUT);
- pinMode(Right_motor_back,OUTPUT);
- pinMode(right_motor_encoder_a,INPUT_PULLUP);
- pinMode(left_motor_encoder_a,INPUT_PULLUP);
- pinMode(right_motor_encoder_b,INPUT);
- pinMode(left_motor_encoder_b,INPUT);
- pinMode(limit_switch,INPUT);
-
- EncoderInit();
- }
- void run() // 前进
- {
- digitalWrite(Right_motor_go,HIGH); // 右电机前进
- digitalWrite(Right_motor_back,LOW);
- analogWrite(Right_motor_go,52);
- analogWrite(Right_motor_back,0);
- digitalWrite(Left_motor_go,HIGH); // 左电机前进
- digitalWrite(Left_motor_back,LOW);
- analogWrite(Left_motor_go,64);
- analogWrite(Left_motor_back,0);
-
- }
- void r_advance()
- {
- // digitalWrite(Right_motor_go,HIGH); // 右电机前进
- // digitalWrite(Right_motor_back,LOW);
- analogWrite(Right_motor_go,80);
- analogWrite(Right_motor_back,0);
- // digitalWrite(Left_motor_go,LOW); // 左电机前进
- // digitalWrite(Left_motor_back,LOW);
- analogWrite(Left_motor_go,0);
- analogWrite(Left_motor_back,30);
- }
- void back()
- {
- digitalWrite(Right_motor_go,LOW); // 右电机前进
- digitalWrite(Right_motor_back,HIGH);
- analogWrite(Right_motor_go,0);
- analogWrite(Right_motor_back,68);
- digitalWrite(Left_motor_go,LOW); // 左电机前进
- digitalWrite(Left_motor_back,HIGH);
- analogWrite(Left_motor_go,0);
- analogWrite(Left_motor_back,80);
- }
- void loop()
- {
- current = millis();
- digitalWrite(outputPin, LOW);
- delayMicroseconds(2);
- digitalWrite(outputPin, HIGH); // Pulse for 10μs to trigger ultrasonic detection
- delayMicroseconds(10);
- digitalWrite(outputPin, LOW);
- const unsigned long duration = pulseIn(inputPin, HIGH);// Read receiver pulse time
- int distance = duration/58 ; // Transform pulse time to distance
- if ( duration == 0 ){
- Serial.println( "Warning: no pulse from sensor");
- }
- else{
- Serial.print ( "distance is:" );
- Serial.println ( distance );
- }
- delay(100);
- if(distance<30){
- back();
- delay(500);
- r_advance();
- delay(300);
- run();
- }
- else{
- run();
- }
- if(distance<10){
- back();
- delay(1000);
- r_advance();
- delay(300);
- run();
- }
- /*int i = analogRead(limit_switch);
- if(i==0){
- back();
- delay(600);
- }*/
-
- if((current=millis())-before > interval)
- {
- before = current;
- Serial.print("left speed is:");
- Serial.println(speed_left);
- Serial.print("right speed is:");
- Serial.println(speed_right);
- speed_left=abs(left_position - left_position_pre)/300;
- speed_right=abs(right_position - right_position_pre)/300;
- left_position_pre = left_position;
- right_position_pre = right_position;
- }
- u8g2.firstPage();
- do {
- u8g2.setFont(u8g2_font_courR12_tr);
- u8g2.setCursor(0,15);
- u8g2.print("distance:");
- u8g2.setCursor(0,30);
- u8g2.print(distance);
- u8g2.print("cm");
- u8g2.setCursor(0,45);
- u8g2.print("l_s:");
- u8g2.print((float)speed_left,2);
- u8g2.setCursor(0,60);
- u8g2.print("r_s:");
- u8g2.print((float)speed_right,2);
- } while ( u8g2.nextPage() );
- delay(1000);
- if(speed_left=0){
- digitalWrite(fm,HIGH);
- }
- else{
- digitalWrite(fm,LOW);
- }
- }
- /*if(speed_left>speed_right){
- r_pwm++;
- }else{
- r_pwm++;
- }
- else if(speed_left=speed_right)
- r_pwm=r_pwm;
- }*/
- void EncoderInit()
- {
- right_Direction = true;//default -> Forward
- pinMode(right_motor_encoder_b,OUTPUT);
- attachInterrupt(digitalPinToInterrupt(2), right_Speed, RISING);
- left_Direction = true;//default -> Forward
- pinMode(left_motor_encoder_b,OUTPUT);
- attachInterrupt(digitalPinToInterrupt(3), left_Speed, RISING);
- }
- void left_Speed()
- {
- //Serial.println("left interrupt is ok");
- int a = digitalRead(left_motor_encoder_a);
- int b = digitalRead(left_motor_encoder_b);
- if((a==HIGH&&b==LOW)||(a==LOW&&b==HIGH)){
- left_position++;
- }
- else if((a==HIGH&&b==HIGH)||(a==LOW&&b==LOW)){
- left_position--;
- }
- }
-
- void right_Speed()
- {
- //Serial.println("right interrupt is ok");
- int c = digitalRead(right_motor_encoder_a);
- int d = digitalRead(right_motor_encoder_b);
- if((c==HIGH&&d==LOW)||(c==LOW&&d==HIGH)){
- right_position++;
- }
- else if((c==HIGH&&d==HIGH)||(c==LOW&&d==LOW)){
- right_position--;
- }
- }
复制代码 自己做的小车 有点丑
PWM.rar
(2.18 KB, 下载次数: 14)
|