zumo
zumo
//ZUMO หาเส้นได้อย่างไร
//ZUMO หาเส้นได้อย่างไร
ZumoReflectanceSensorArray reflectanceSensors;ZumoMotors motors;Pushbutton button(ZUMO_BUTTON);int lastError = 0;const int MAX_SPEED = 200;void setup(){unsigned int sensors[6];unsigned short count = 0;unsigned short last_status = 0;int turn_direction = 1;pinMode(13, OUTPUT);reflectanceSensors.init();digitalWrite(13, HIGH); delay(200); digitalWrite(13, LOW); delay(200);digitalWrite(13, HIGH); delay(200); digitalWrite(13, LOW); delay(200);digitalWrite(13, HIGH); delay(200); digitalWrite(13, LOW); delay(200);button.waitForButton();digitalWrite(13, HIGH); delay(1000);for (int i = 0; i < 4; i ++){turn_direction *= -1;motors.setSpeeds(turn_direction * TURN_SPEED, -1 * turn_direction * TURN_SPEED);while (count < 2){reflectanceSensors.calibrate();reflectanceSensors.readLine(sensors);if (turn_direction < 0){count += ABOVE_LINE(sensors[5]) ^ last_status;last_status = ABOVE_LINE(sensors[5]);}else {count += ABOVE_LINE(sensors[0]) ^ last_status;last_status = ABOVE_LINE(sensors[0]);}}count = 0;last_status = 0;}// turn('L');motors.setSpeeds(0, 0);// หยุดมอเตอร์digitalWrite(13, LOW);button.waitForButton();// รอการกดสวิทช์}//void loop(){}
//ZUMO เคลื่อนที่ตามเส้นได้อย่างไร
//ZUMO เคลื่อนที่ตามเส้นได้อย่างไร
void Trackline(int MAX_SPEED){unsigned int sensors[6];int position = reflectanceSensors.readLine(sensors);int error = position - 2500;int speedDifference = error / 4 + 6 * (error - lastError);lastError = error;int m1Speed = MAX_SPEED + speedDifference;int m2Speed = MAX_SPEED - speedDifference;if (m1Speed < 0)m1Speed = 0;if (m2Speed < 0)m2Speed = 0;if (m1Speed > MAX_SPEED)m1Speed = MAX_SPEED;if (m2Speed > MAX_SPEED)m2Speed = MAX_SPEED;motors.setSpeeds(m1Speed, m2Speed);}
//ZUMO เลี้ยวได้อย่างไร
//ZUMO เลี้ยวได้อย่างไร
// count and last_status help// keep track of how much further// the Zumo needs to turn.unsigned short count = 0;unsigned short last_status = 0;unsigned int sensors[6];// dir tests for which direction to turnswitch(dir){// Since we're using the sensors to coordinate turns instead of timing them, // we can treat a left turn the same as a direction reversal: they differ only // in whether the zumo will turn 90 degrees or 180 degrees before seeing the // line under the sensor. If 'B' is passed to the turn function when there is a// left turn available, then the Zumo will turn onto the left segment.case 'L':case 'B':// Turn left.motors.setSpeeds(-TURN_SPEED, TURN_SPEED);// This while loop monitors line position// until the turn is complete. while(count < 2){reflectanceSensors.readLine(sensors);// Increment count whenever the state of the sensor changes // (white->black and black->white) since the sensor should // pass over 1 line while the robot is turning, the final // count should be 2count += ABOVE_LINE(sensors[1]) ^ last_status; last_status = ABOVE_LINE(sensors[1]);}break;case 'R':// Turn right.motors.setSpeeds(TURN_SPEED, -TURN_SPEED);// This while loop monitors line position// until the turn is complete. while(count < 2){reflectanceSensors.readLine(sensors);count += ABOVE_LINE(sensors[4]) ^ last_status;last_status = ABOVE_LINE(sensors[4]);}break;case 'S':// Don't do anything!break;}}
//ZUMO นับจำนวนแยกในการเคลื่อนที่ได้อย่างไร
//ZUMO นับจำนวนแยกในการเคลื่อนที่ได้อย่างไร
//กรณีสี่แยก
void lineCross(int count){int j = 0; // กำหนดค่าเริ่มต้นในการนับwhile (j < count) // ตรวจสอบจำนวนแยกที่นับได้{Trackline(300); // เดินตามเส้นunsigned int sensors[6];int position = reflectanceSensors.readLine(sensors);if (ABOVE_LINE(sensors[0]) && ABOVE_LINE(sensors[5])) // กรณีเจอแยก{int position = reflectanceSensors.readLine(sensors);while (ABOVE_LINE(sensors[0]) && ABOVE_LINE(sensors[5])) // เดินหน้าจนกว่าจะเจอสีขาว{
int position = reflectanceSensors.readLine(sensors);motors.setSpeeds(200, 200); // ฟังก์ชันเดินหน้าdelay (100); // หน่วงเวลา}j++; // เพิ่มค่าขึ้น 1 กรณีเจอเส้นด า}//delay(100); // หน่วงเวลา 0.1 วินาที}//delay(100); // หน่วงเวลาให้หุ่นยนต์หยุดกลางเส้นmotors.setSpeeds(0, 0); // หยุดมอเตอร์}
//กรณีแยกซ้าย
void lineCrossLeft(int count){int j = 0; // กำหนดค่าเริ่มต้นในการนับwhile (j < count) // ตรวจสอบจำนวนแยกที่นับได้{Trackline(300); // เดินตามเส้นunsigned int sensors[6];int position = reflectanceSensors.readLine(sensors);if (ABOVE_LINE(sensors[0])) // กรณีเจอแยก{int position = reflectanceSensors.readLine(sensors);while (ABOVE_LINE(sensors[0])) // เดินหน้าจนกว่าจะเจอสีขาว{
int position = reflectanceSensors.readLine(sensors);motors.setSpeeds(200, 200); // ฟังก์ชันเดินหน้าdelay (100); // หน่วงเวลา}j++; // เพิ่มค่าขึ้น 1 กรณีเจอเส้นดำ}//delay(100); // หน่วงเวลา 0.1 วินาที}//delay(100); // หน่วงเวลาให้หุ่นยนต์หยุดกลางเส้นmotors.setSpeeds(0, 0); // หยุดมอเตอร์}
//กรณีแยกขวา
void lineCrossRight(int count){int j = 0; // กำหนดค่าเริ่มต้นในการนับwhile (j < count) // ตรวจสอบจำนวนแยกที่นับได้{Trackline(300); // เดินตามเส้นunsigned int sensors[6];int position = reflectanceSensors.readLine(sensors);if (ABOVE_LINE(sensors[5])) // กรณีเจอแยก{int position = reflectanceSensors.readLine(sensors);while (ABOVE_LINE(sensors[5])) // เดินหน้าจนกว่าจะเจอสีขาว{
int position = reflectanceSensors.readLine(sensors);motors.setSpeeds(200, 200); // ฟังก์ชันเดินหน้าdelay (100); // หน่วงเวลา}j++; // เพิ่มค่าขึ้น 1 กรณีเจอเส้นดำ}//delay(500); // หน่วงเวลา 0.1 วินาที}//delay(500); // หน่วงเวลาให้หุ่นยนต์หยุดกลางเส้นmotors.setSpeeds(0, 0); // หยุดมอเตอร์}
//ชนกระป๋อง
void crash(int count)
{
int j = 0; // กำหนดค่าเริ่มต้นในการนับ
while (j < count) // ตรวจสอบจำนวนแยกที่นับได้
{
Trackline(300); // เดินตามเส้น
unsigned int sensors[6];
int position = reflectanceSensors.readLine(sensors);
if (position ==0) // กรณีเจอแยก
{
int position = reflectanceSensors.readLine(sensors);
motors.setSpeeds(0, 0);delay (1000);
/*
while (position == 0) // เดินหน้าจนกว่าจะเจอสีขาว
{
int position = reflectanceSensors.readLine(sensors);
motors.setSpeeds(200, 200); // ฟังก์ชันเดินหน้า
delay (100); // หน่วงเวลา
} */
j++; // เพิ่มค่าขึ้น 1 กรณีเจอเส้นด า
}
//delay(100); // หน่วงเวลา 0.1 วินาที
}
//delay(100); // หน่วงเวลาให้หุ่นยนต์หยุดกลางเส้น
motors.setSpeeds(0, 0); // หยุดมอเตอร์
}
ภารกิจ
#include <QTRSensors.h>#include <ZumoReflectanceSensorArray.h>#include <ZumoMotors.h>#include <ZumoBuzzer.h>#include <Pushbutton.h>#define SENSOR_THRESHOLD 300#define ABOVE_LINE(sensor)((sensor) > SENSOR_THRESHOLD)#define TURN_SPEED 200
ZumoReflectanceSensorArray reflectanceSensors;ZumoMotors motors;Pushbutton button(ZUMO_BUTTON);int lastError = 0;const int MAX_SPEED = 200;
void setup(){unsigned int sensors[6];unsigned short count = 0;unsigned short last_status = 0;int turn_direction = 1;pinMode(13, OUTPUT);reflectanceSensors.init();
digitalWrite(13, HIGH); delay(200); digitalWrite(13, LOW); delay(200);digitalWrite(13, HIGH); delay(200); digitalWrite(13, LOW); delay(200);digitalWrite(13, HIGH); delay(200); digitalWrite(13, LOW); delay(200);button.waitForButton();
digitalWrite(13, HIGH);delay(1000);
// Calibrate the Zumo by sweeping it from left to rightfor (int i = 0; i < 4; i ++){// Zumo will turn clockwise if turn_direction = 1.// If turn_direction = -1 Zumo will turn counter-clockwise.turn_direction *= -1;
// Turn direction.motors.setSpeeds(turn_direction * TURN_SPEED, -1 * turn_direction * TURN_SPEED);
// This while loop monitors line position// until the turn is complete.while (count < 2){reflectanceSensors.calibrate();reflectanceSensors.readLine(sensors);if (turn_direction < 0){// If the right most sensor changes from (over white space -> over// line or over line -> over white space) add 1 to count.count += ABOVE_LINE(sensors[5]) ^ last_status;last_status = ABOVE_LINE(sensors[5]);}else{// If the left most sensor changes from (over white space -> over// line or over line -> over white space) add 1 to count.count += ABOVE_LINE(sensors[0]) ^ last_status;last_status = ABOVE_LINE(sensors[0]);}}
count = 0;last_status = 0;}
// Turn left.turn('L');motors.setSpeeds(0, 0);// Turn off LED to indicate we are through with calibrationdigitalWrite(13, LOW);// Wait for the user button to be pressed and released
button.waitForButton();
lineCrossRight(1);turn('R');crash(1);turn('L');lineCross(1);turn('L');lineCrossLeft(1);turn('L');crash(1);turn('R');lineCross(1);turn('L');turn('R');turn('R');turn('L');lineCross(1);motors.setSpeeds(0, 0);
}
void loop(){
//turn('R');//motors.setSpeeds(0, 0);//delay(1000);
//Trackline(200);
}
void Trackline(int MAX_SPEED){unsigned int sensors[6];int position = reflectanceSensors.readLine(sensors);int error = position - 2500;int speedDifference = error / 4 + 6 * (error - lastError);lastError = error;int m1Speed = MAX_SPEED + speedDifference;int m2Speed = MAX_SPEED - speedDifference;
if (m1Speed < 0)m1Speed = 0;if (m2Speed < 0)m2Speed = 0;if (m1Speed > MAX_SPEED)m1Speed = MAX_SPEED;if (m2Speed > MAX_SPEED)m2Speed = MAX_SPEED;motors.setSpeeds(m1Speed, m2Speed);}
void lineCross(int count){int j = 0; // กำหนดค่าเริ่มต้นในการนับwhile (j < count) // ตรวจสอบจำนวนแยกที่นับได้{Trackline(300); // เดินตามเส้นunsigned int sensors[6];int position = reflectanceSensors.readLine(sensors);if (ABOVE_LINE(sensors[0]) && ABOVE_LINE(sensors[5])) // กรณีเจอแยก{int position = reflectanceSensors.readLine(sensors);
while (ABOVE_LINE(sensors[0]) && ABOVE_LINE(sensors[5])) // เดินหน้าจนกว่าจะเจอสีขาว{
int position = reflectanceSensors.readLine(sensors);motors.setSpeeds(200, 200); // ฟังก์ชันเดินหน้าdelay (100); // หน่วงเวลา}j++; // เพิ่มค่าขึ้น 1 กรณีเจอเส้นด า}//delay(100); // หน่วงเวลา 0.1 วินาที}//delay(100); // หน่วงเวลาให้หุ่นยนต์หยุดกลางเส้นmotors.setSpeeds(0, 0); // หยุดมอเตอร์}
void lineCrossLeft(int count){int j = 0; // กำหนดค่าเริ่มต้นในการนับwhile (j < count) // ตรวจสอบจำนวนแยกที่นับได้{Trackline(300); // เดินตามเส้นunsigned int sensors[6];int position = reflectanceSensors.readLine(sensors);if (ABOVE_LINE(sensors[0])) // กรณีเจอแยก{int position = reflectanceSensors.readLine(sensors);
while (ABOVE_LINE(sensors[0])) // เดินหน้าจนกว่าจะเจอสีขาว{
int position = reflectanceSensors.readLine(sensors);motors.setSpeeds(200, 200); // ฟังก์ชันเดินหน้าdelay (100); // หน่วงเวลา}j++; // เพิ่มค่าขึ้น 1 กรณีเจอเส้นดำ}//delay(100); // หน่วงเวลา 0.1 วินาที}//delay(100); // หน่วงเวลาให้หุ่นยนต์หยุดกลางเส้นmotors.setSpeeds(0, 0); // หยุดมอเตอร์}void lineCrossRight(int count){int j = 0; // กำหนดค่าเริ่มต้นในการนับwhile (j < count) // ตรวจสอบจำนวนแยกที่นับได้{Trackline(300); // เดินตามเส้นunsigned int sensors[6];int position = reflectanceSensors.readLine(sensors);if (ABOVE_LINE(sensors[5])) // กรณีเจอแยก{int position = reflectanceSensors.readLine(sensors);
while (ABOVE_LINE(sensors[5])) // เดินหน้าจนกว่าจะเจอสีขาว{
int position = reflectanceSensors.readLine(sensors);motors.setSpeeds(200, 200); // ฟังก์ชันเดินหน้าdelay (100); // หน่วงเวลา}j++; // เพิ่มค่าขึ้น 1 กรณีเจอเส้นดำ}//delay(500); // หน่วงเวลา 0.1 วินาที}//delay(500); // หน่วงเวลาให้หุ่นยนต์หยุดกลางเส้นmotors.setSpeeds(0, 0); // หยุดมอเตอร์}
void turn(char dir){
// count and last_status help// keep track of how much further// the Zumo needs to turn.unsigned short count = 0;unsigned short last_status = 0;unsigned int sensors[6];
// dir tests for which direction to turnswitch (dir){
// Since we're using the sensors to coordinate turns instead of timing them,// we can treat a left turn the same as a direction reversal: they differ only// in whether the zumo will turn 90 degrees or 180 degrees before seeing the// line under the sensor. If 'B' is passed to the turn function when there is a// left turn available, then the Zumo will turn onto the left segment.case 'L':case 'B':// Turn left.motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
// This while loop monitors line position// until the turn is complete.while (count < 2){reflectanceSensors.readLine(sensors);
// Increment count whenever the state of the sensor changes// (white->black and black->white) since the sensor should// pass over 1 line while the robot is turning, the final// count should be 2count += ABOVE_LINE(sensors[1]) ^ last_status;last_status = ABOVE_LINE(sensors[1]);}
break;
case 'R':// Turn right.motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
// This while loop monitors line position// until the turn is complete.while (count < 2){reflectanceSensors.readLine(sensors);count += ABOVE_LINE(sensors[4]) ^ last_status;last_status = ABOVE_LINE(sensors[4]);}
break;
case 'S':// Don't do anything!break;}}
void crash(int count){int j = 0; // กำหนดค่าเริ่มต้นในการนับwhile (j < count) // ตรวจสอบจำนวนแยกที่นับได้{Trackline(300); // เดินตามเส้นunsigned int sensors[6];int position = reflectanceSensors.readLine(sensors);if (position ==0) // กรณีเจอแยก{int position = reflectanceSensors.readLine(sensors);motors.setSpeeds(0, 0);delay (1000);/*while (position == 0) // เดินหน้าจนกว่าจะเจอสีขาว{int position = reflectanceSensors.readLine(sensors);motors.setSpeeds(200, 200); // ฟังก์ชันเดินหน้าdelay (100); // หน่วงเวลา} */j++; // เพิ่มค่าขึ้น 1 กรณีเจอเส้นด า}//delay(100); // หน่วงเวลา 0.1 วินาที}//delay(100); // หน่วงเวลาให้หุ่นยนต์หยุดกลางเส้นmotors.setSpeeds(0, 0); // หยุดมอเตอร์}