zumo

//ZUMO หาเส้นได้อย่างไร


#include <QTRSensors.h>#include <ZumoReflectanceSensorArray.h>#include <ZumoMotors.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);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 เคลื่อนที่ตามเส้นได้อย่างไร



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 เลี้ยวได้อย่างไร


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;}}

//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); // หยุดมอเตอร์}