Lab 4 การสั่งงาน DC Motor
ต่อมอเตอร์เข้าไปที่ Nano Shield ทั้ง 2 ตัวดังภาพ
ภาพการต่อวงจรของ Zack Robot Nano Shield
Arduino IDE Code
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define DR1 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7
#define DR2 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8
#define PWMR 6 /// กำหนดสัญญาณ PWM ขวาพอร์ต 6
/////////////////////////////////
#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4
#define PWML 5 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 5
void setup() {
Serial.begin(9600);
//////////////////////////////////////////////////
pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT
////////กำหนดสัญญาณทุกขาเป็น OUTPUT ///////////////
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
////////////////////////////
}
void loop() {
int sw = digitalRead(button);
if (sw == 0) {
tone(3, 1000, 500);
///////////////// เขียน Code ทดสอบที่นี่ /////////////
run(100, 100); delay(1000); run(0, 0);tone(3,1200,100);
run(100, 0); delay(1000); run(0, 0);tone(3,1200,100);
run(-100, -100); delay(1000); run(0, 0);tone(3,1200,100);
run(0, 100); delay(1000); run(0, 0);tone(3,1200,100);
}
///////////////////////////////////////////////
else {
run(0, 0);
}
Serial.println(sw);
}
////////////////////////////////////////
void run(int spl, int spr) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{
if (spl > 0)
{
digitalWrite(DL1, LOW);
digitalWrite(DL2, HIGH);
analogWrite(PWML, spl);
}
else if (spl < 0)
{
digitalWrite(DL1, HIGH);
digitalWrite(DL2, LOW);
analogWrite(PWML, -spl);
}
else
{
digitalWrite(DL1, LOW);
digitalWrite(DL2, LOW);
analogWrite(PWML, -255);
}
//////////////////////////////////////
if (spr > 0)
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, spr);
}
else if (spr < 0)
{
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -spr);
}
else
{
digitalWrite(DR1, LOW);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -255);
}
}
สำหรับบอร์ด Phurawong Robot Team
Arduino IDE Code
#include <SPI.h>
#include <Wire.h>
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define display_RESET 4
Adafruit_SSD1306 display(display_RESET);
Servo keepservo;
Servo upservo;
//Servo sv3;
//#define PWML 5 // motor L
#define IN1L 3 //
#define IN2L 5 //
//#define PWMR 6 // motor R
#define IN1R 6 //
#define IN2R 11 //
#define buttonPin 2
void setup() {
/////////////////////////////////////////
/////////// ช่วงกำหนด pin งดเปลี่ยนแปลง////////
// pinMode(PWML,OUTPUT);
pinMode(IN1L,OUTPUT);
pinMode(IN2L,OUTPUT);
// pinMode(PWMR,OUTPUT);
pinMode(IN1R,OUTPUT);
pinMode(IN2R,OUTPUT);
pinMode(buttonPin, INPUT);
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3D (for the 128x64)
}
//////////////////////////////////////////////////////////
////////////////////// loop ////////////////////////////////////
void loop()
{
int sw = digitalRead(buttonPin); // ให้ sw อ่านค่า digital จากพอร์ต 2(button)
if (sw==1) {run(70,70);delay(500);run(-70,70);delay(500);run(0,0);delay(500);}
}
// trackT();
// left();
//Motor(0,80); delay(500);
////////////////////////////////////////////////////////////////////////////
void run(int sl,int sr){
if((sl==0) and (sr==0)){
digitalWrite(IN1L,HIGH);
digitalWrite(IN1R,HIGH);
digitalWrite(IN2L,HIGH);
digitalWrite(IN2R,HIGH);
delay(100);
digitalWrite(IN1L,LOW);
digitalWrite(IN1R,LOW);
digitalWrite(IN2L,LOW);
digitalWrite(IN2R,LOW);
}
if(sr>255)
sr = 255;
else if(sr<-255)
sr = -255;
if(sl>255)
sl = 255;
else if(sl<-255)
sl = -255;
if(sl>0){
analogWrite(IN1L,sl);
digitalWrite(IN2L,LOW);
// analogWrite(PWML,sl);
}else if(sl<0){
digitalWrite(IN1L,LOW);
analogWrite(IN2L,-sl);
// analogWrite(PWML,-sl);
}else{
digitalWrite(IN1L,HIGH);
digitalWrite(IN2L,HIGH);
// analogWrite(PWML,50);
}
if(sr>0){
analogWrite(IN1R,sr);
digitalWrite(IN2R,LOW);
// analogWrite(PWMR,sr);
}else if(sr<0){
digitalWrite(IN1R,LOW);
analogWrite(IN2R,-sr);
// analogWrite(PWMR,-sr);
}else{
digitalWrite(IN1R,HIGH);
digitalWrite(IN2R,HIGH);
// analogWrite(PWMR,50);
}
}
///////////////////////////////////////////////////////////////////////
//
// ฟังก์ชั่นอ่านค่า Analog
//
///////////////////////////////////////////////////////////////////////