From 630ffd35a7e7a0b88320465fd669ef33e9cdd20c Mon Sep 17 00:00:00 2001 From: David Cabeza Date: Mon, 3 Dec 2018 15:59:39 +0100 Subject: [PATCH 01/38] preliminar version of u-turn functions --- arduino-maze-robot.ino | 30 ++++++++++++++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/arduino-maze-robot.ino b/arduino-maze-robot.ino index d614219..513f699 100644 --- a/arduino-maze-robot.ino +++ b/arduino-maze-robot.ino @@ -20,11 +20,11 @@ const int right_motorPin = 12; #define NUM_SENSORS 6 //number of sensors used #define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low -#define EMITTER_PIN 7 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 +#define EMITTER_PIN 8 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 #define DEBUG 1 // sensors are connected to digital pins 1 to 6 -QTRSensorsRC qtrrc((unsigned char[]) {1, 2, 3, 4, 5, 6}, +QTRSensorsRC qtrrc((unsigned char[]) {2, 3, 4, 5, 6, 7}, NUM_SENSORS, TIMEOUT, EMITTER_PIN); unsigned int sensorValues[NUM_SENSORS]; @@ -170,3 +170,29 @@ void print_left_right_motorSpeed(int leftMotorSpeed, int rightMotorSpeed){ Serial.println(rightMotorSpeed); return; } + +bool reflectance_dead_end(int position){ + int lower_bound = 2300; + int higher_bound = 2700; + if (position > 2300 and position < 2700){ + return true; + } + return false; +} + +void check_u_turn(int position){ + u_turn = false; // we assume we shouldn't u-turn + // stage1: check for reflectance bounds in a + u_turn = reflectance_dead_end(position); + + return u_turn; +} + +void u_turn(){ + for (int i=0; i<22500;i++){ + motor1.write(0); + motor2.write(0); + } + set_motors(90, 90); + return; +} \ No newline at end of file From b2aa3be5300c98f4e887d34e57fe850ca0c0827f Mon Sep 17 00:00:00 2001 From: David Cabeza Date: Mon, 3 Dec 2018 16:01:40 +0100 Subject: [PATCH 02/38] improvements to u-turn comments and code --- arduino-maze-robot.ino | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/arduino-maze-robot.ino b/arduino-maze-robot.ino index 513f699..52298d4 100644 --- a/arduino-maze-robot.ino +++ b/arduino-maze-robot.ino @@ -171,7 +171,7 @@ void print_left_right_motorSpeed(int leftMotorSpeed, int rightMotorSpeed){ return; } -bool reflectance_dead_end(int position){ +bool check_reflectance_dead_end(int position){ int lower_bound = 2300; int higher_bound = 2700; if (position > 2300 and position < 2700){ @@ -181,9 +181,10 @@ bool reflectance_dead_end(int position){ } void check_u_turn(int position){ - u_turn = false; // we assume we shouldn't u-turn - // stage1: check for reflectance bounds in a - u_turn = reflectance_dead_end(position); + u_turn = false; // we assume we shouldn't do u-turn + // stage1: check for reflectance bounds in a fixed range (determined by testing) + u_turn = check_reflectance_dead_end(position); + // stage2: check for ultra-sound sensor (maybe a variable that has been set before) return u_turn; } @@ -193,6 +194,6 @@ void u_turn(){ motor1.write(0); motor2.write(0); } - set_motors(90, 90); + //set_motors(90, 90); return; } \ No newline at end of file From c182e4480dae68af1a2d88b649768ef359a9162c Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 4 Dec 2018 14:48:31 +0100 Subject: [PATCH 03/38] Update arduino-maze-robot.ino --- arduino-maze-robot.ino | 45 +++++++++++++++++++++++++++++++----------- 1 file changed, 33 insertions(+), 12 deletions(-) diff --git a/arduino-maze-robot.ino b/arduino-maze-robot.ino index 52298d4..969becc 100644 --- a/arduino-maze-robot.ino +++ b/arduino-maze-robot.ino @@ -1,5 +1,5 @@ /* - * + 容纳graspping system的控制函数和line following的控制函数 */ #include #include // Pololu QTR Sensor Library. @@ -21,9 +21,9 @@ const int right_motorPin = 12; #define NUM_SENSORS 6 //number of sensors used #define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low #define EMITTER_PIN 8 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 -#define DEBUG 1 +#define DEBUG 0 // 设置为1后，方便后面degbug时要不要打印出来传感器等的参数 -// sensors are connected to digital pins 1 to 6 +// sensors are connected to digital pins 2 to 7 QTRSensorsRC qtrrc((unsigned char[]) {2, 3, 4, 5, 6, 7}, NUM_SENSORS, TIMEOUT, EMITTER_PIN); @@ -42,33 +42,34 @@ void setup() { } void loop() { - int position = qtrrc.readLine(sensorValues); //get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position. + int position = qtrrc.readLine(sensorValues); + //get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position. if (DEBUG) print_sensor_values(position); int error = position - 2500; - //if (DEBUG) print_error(error); + if (DEBUG) print_error(error); int motorSpeed = KP * error + KD * (error - lastError); - //if (DEBUG) print_motorSpeed(motorSpeed); + if (DEBUG) print_motorSpeed(motorSpeed); lastError = error; int leftMotorSpeed = 180 - left_motor_default_speed - motorSpeed; // closer to 0, speed up int rightMotorSpeed = right_motor_default_speed - motorSpeed; // closer to 180, speed up, so slow down here - //if (DEBUG) print_left_right_motorSpeed(leftMotorSpeed, rightMotorSpeed); + if (DEBUG) print_left_right_motorSpeed(leftMotorSpeed, rightMotorSpeed); // set motor speeds using the two motor speed variables above // limit their speed to 90~180 set_motors(leftMotorSpeed, rightMotorSpeed); - //set_motors(90, 90); - //set_motors(0, 180); + //set_motors(90, 90); // stop } void set_motors(int left_motorspeed, int right_motorspeed) { + // limit motor's speed to 0~90, 90~180 if (180 - left_motorspeed > left_motor_max_speed ) left_motorspeed = 180 - left_motor_max_speed; // if speed is negative, use 0 instead if (right_motorspeed > right_motor_max_speed ) right_motorspeed = right_motor_max_speed; if ( left_motorspeed > 90) left_motorspeed = 90; // avoid spinning backward @@ -139,6 +140,7 @@ void manual_calibration() { } void print_sensor_values(int position){ + // for the use of calibration and debug // print the sensor values as numbers from 0 to 1000, where 0 means maximum reflectance and // 1000 means minimum reflectance, followed by the line position for (unsigned char i = 0; i < NUM_SENSORS; i++){ @@ -146,13 +148,14 @@ void print_sensor_values(int position){ Serial.print('\t'); } //Serial.println(); // uncomment this line if you are using raw values - //String pos = "Position: "; - //String fpos = pos + position; + String pos = "Position: "; + String fpos = pos + position; Serial.println(position); // comment this line out if you are using raw values return; } void print_error(int error){ + // for the use of debug String err = "Error: "; String ferr = err + error; Serial.println(ferr); @@ -196,4 +199,22 @@ void u_turn(){ } //set_motors(90, 90); return; -} \ No newline at end of file +} + +void right_turn(){ + for (int i=0; i<22500/2;i++){ + motor1.write(0); // 左轮向前，右轮倒转，是为右转 + motor2.write(180); + } + //set_motors(90, 90); + return; +} + +void left_turn(){ + for (int i=0; i<22500/2;i++){ + motor1.write(180); // 左轮向后，右轮向前，是为左转 + motor2.write(0); + } + //set_motors(90, 90); + return; +} From b502b7cb2fb8b5c4b2f514d5ee2a2a2b0ed0e2b8 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Wed, 5 Dec 2018 14:02:45 +0100 Subject: [PATCH 04/38] Landscape Judgment MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit With the combination of the reflectance sensors and ultrasound sensors to judge 6 types of landscape:1, 2, 3, 4, 5, 6 corresponds to leftonly, rightonly, T, 十, leftandstraight. Dead end's judgment is not included yet. --- sketch_landscape.ino | 340 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 340 insertions(+) create mode 100644 sketch_landscape.ino diff --git a/sketch_landscape.ino b/sketch_landscape.ino new file mode 100644 index 0000000..4824099 --- /dev/null +++ b/sketch_landscape.ino @@ -0,0 +1,340 @@ +/* + 容纳graspping system的控制函数和line following的控制函数 + */ +#include +#include // Pololu QTR Sensor Library. + +Servo left_motor, right_motor; +Servo gripper, lifter; +const int lifterPin = 9; +const int gripperPin = 10; +const int left_motorPin = 11; +const int right_motorPin = 12; + +#define KP 0.2 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed +#define KD 0.5 //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd) +#define left_motor_default_speed 180 //normal speed of the left_motor +#define right_motor_default_speed 180 //normal speed of the right_motor +#define left_motor_max_speed 180 //max. speed of the left_motor +#define right_motor_max_speed 180 //max. speed of the right_motor + +#define NUM_SENSORS 6 //number of sensors used +#define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low +#define EMITTER_PIN 8 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 +#define DEBUG 0 // 设置为1后，方便后面degbug时要不要打印出来传感器等的参数 + +// sensors are connected to digital pins 2 to 7 +QTRSensorsRC qtrrc((unsigned char[]) {2, 3, 4, 5, 6, 7}, + NUM_SENSORS, TIMEOUT, EMITTER_PIN); + +unsigned int sensorValues[NUM_SENSORS]; +int lastError = 0; + +void setup() { + left_motor.attach(left_motorPin); + right_motor.attach(right_motorPin); + gripper.attach(gripperPin); + lifter.attach(lifterPin); + set_motors(90, 90); // set the motors speed zero. + init_gripper(); + init_lifter(); + manual_calibration(); +} + +void loop() { + int position = qtrrc.readLine(sensorValues); + //get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position. + + if (DEBUG) print_sensor_values(position); + + int error = position - 2500; + + if (DEBUG) print_error(error); + + int motorSpeed = KP * error + KD * (error - lastError); + + if (DEBUG) print_motorSpeed(motorSpeed); + + lastError = error; + + int leftMotorSpeed = 180 - left_motor_default_speed - motorSpeed; // closer to 0, speed up + int rightMotorSpeed = right_motor_default_speed - motorSpeed; // closer to 180, speed up, so slow down here + + if (DEBUG) print_left_right_motorSpeed(leftMotorSpeed, rightMotorSpeed); + + // set motor speeds using the two motor speed variables above + // limit their speed to 90~180 + set_motors(leftMotorSpeed, rightMotorSpeed); + //set_motors(90, 90); // stop +} + +void set_motors(int left_motorspeed, int right_motorspeed) { + // limit motor's speed to 0~90, 90~180 + if (180 - left_motorspeed > left_motor_max_speed ) left_motorspeed = 180 - left_motor_max_speed; // if speed is negative, use 0 instead + if (right_motorspeed > right_motor_max_speed ) right_motorspeed = right_motor_max_speed; + if ( left_motorspeed > 90) left_motorspeed = 90; // avoid spinning backward + if (right_motorspeed < 90) right_motorspeed = 90; + //left_motorspeed = 180 - left_motorspeed; + left_motor.write(left_motorspeed); + right_motor.write(right_motorspeed); +} + +void init_lifter(){ + lifter.write(180); + return; +} + +void lift(){ + lifter.write(35); + return; +} + +void unlift(){ + lifter.write(180); + return; +} + +void init_gripper(){ + gripper.write(0); + return; +} + +void open_gripper(){ + gripper.write(0); + return; +} + +void close_gripper(){ + gripper.write(35); + return; +} + +void manual_calibration() { + //calibrate for sometime by sliding the sensors across the line, + //or you may use auto-calibration instead + + pinMode(13, OUTPUT); + digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode + //void emittersOn(); + for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds + { + qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call) + } + digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration + + // print the calibration minimum values measured when emitters were on + Serial.begin(9600); + for (int i = 0; i < NUM_SENSORS; i++){ + Serial.print(qtrrc.calibratedMinimumOn[i]); + Serial.print(' '); + } + Serial.println(); + + // print the calibration maximum values measured when emitters were on + for (int i = 0; i < NUM_SENSORS; i++){ + Serial.print(qtrrc.calibratedMaximumOn[i]); + Serial.print(' '); + } + Serial.println(); + // delay(1000); +} + +void print_sensor_values(int position){ + // for the use of calibration and debug + // print the sensor values as numbers from 0 to 1000, where 0 means maximum reflectance and + // 1000 means minimum reflectance, followed by the line position + for (unsigned char i = 0; i < NUM_SENSORS; i++){ + Serial.print(sensorValues[i]); + Serial.print('\t'); + } + //Serial.println(); // uncomment this line if you are using raw values + String pos = "Position: "; + String fpos = pos + position; + Serial.println(position); // comment this line out if you are using raw values + return; +} + +void print_error(int error){ + // for the use of debug + String err = "Error: "; + String ferr = err + error; + Serial.println(ferr); + Serial.println(); + return; +} + +void print_motorSpeed(int motorSpeed){ + Serial.println(motorSpeed); + return; +} + +void print_left_right_motorSpeed(int leftMotorSpeed, int rightMotorSpeed){ + Serial.println(leftMotorSpeed); + Serial.println(rightMotorSpeed); + return; +} + +bool check_reflectance_dead_end(int position){ + int lower_bound = 2300; + int higher_bound = 2700; + if (position > 2300 and position < 2700){ + return true; + } + return false; +} + +void check_u_turn(int position){ + u_turn = false; // we assume we shouldn't do u-turn + // stage1: check for reflectance bounds in a fixed range (determined by testing) + u_turn = check_reflectance_dead_end(position); + // stage2: check for ultra-sound sensor (maybe a variable that has been set before) + + return u_turn; // 可以返回函数？？？ +} + +void u_turn(){ + for (int i=0; i<22500;i++){ + motor1.write(0); + motor2.write(0); + } + //set_motors(90, 90); + return; +} + +void right_turn(){ + for (int i=0; i<22500/2;i++){ + motor1.write(0); // 左轮向前，右轮倒转，是为右转 + motor2.write(180); + } + //set_motors(90, 90); + return; +} + +void left_turn(){ + for (int i=0; i<22500/2;i++){ + motor1.write(180); // 左轮向后，右轮向前，是为左转 + motor2.write(0); + } + //set_motors(90, 90); + return; +} + +int landscape(){ + // first make sure the car is online, then judge the landscape + // use ultra sound sensors to distinguish 'only right' and 'right or straight' + // also for 'T' and '十' + //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, + //rightandstraight + if(online_judge){ + delay(1000); // experiment to determine this + if(onlyleft_judge) return 1; + else return 5; + + if(onlyright_judge) return 2; + else return 6; + + if(T_judge) return 3; + else return 4; + } + + } + +bool sensor_judge(num, range, erro){ + // experiment to determine if sensorValues[0] is exact the 1st sensor!!! + if (sensorValues[num] < range + erro && sensorValues[num] > range - erro) return true; + else return false; + } + +bool online_judge(){ + // first judge if the raw vaues array is 0 0 1000 1000 0 0 + // erro is ±250, change it after experiment + for (unsigned char i = 0;i < NUM_SENSORS; i++){ + if (i != 2 or 3){ + unsigned int range = 0; + unsigned int erro = 250; + if (sensor_judge(i, range, erro) == false) { + return false; + } + } + else{ + unsigned int range = 1000; + unsigned int erro = 250; + if (sensor_judge(i, range, erro) == false){ + return false; + } + } + return true; + } + } + +bool onlyleft_judge(){ + // tell onlyleft from 'left and straight' + // the sensor values should be {1000, 1000, 1000, 1000, 0, 0} + for (unsigned char i = 0;i < NUM_SENSORS; i++){ + if (i != 4 or 5){ + unsigned int range = 1000; + unsigned int erro = 250; + if (sensor_judge(i, range, erro) == false) { + return false; + } + } + else{ + unsigned int range = 0; + unsigned int erro = 250; + if (sensor_judge(i, range, erro) == false){ + return false; + } + } + return true; + } + } + +bool onlyright_judge(){ + // tell onlylright from 'right and straight' + // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} + for (unsigned char i = 0;i < NUM_SENSORS; i++){ + if (i != 1 or 2 ){ + unsigned int range = 10000; + unsigned int erro = 250; + if (sensor_judge(i, range, erro) == false) { + return false; + } + } + else{ + unsigned int range = 0; + unsigned int erro = 250; + if (sensor_judge(i, range, erro) == false){ + return false; + } + } + return true; + } + } + +bool T_judge(){ + // tell T from 十 + // the sensor values should be all 1000 + for (unsigned char i = 0;i < NUM_SENSORS; i++){ + if (i != 1 or 2 ){ + unsigned int range = 10000; + unsigned int erro = 250; + if (sensor_judge(i, range, erro) == false) { + return false; + } + } + else{ + unsigned int range = 0; + unsigned int erro = 250; + if (sensor_judge(i, range, erro) == false){ + return false; + } + } + return true; + } + } + + +bool wall_judge(){ + // use ultra_sound to judge if there is a wall in fornt + + } From fdd61fafc736101d3b2efb5398a283a4d877b014 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Wed, 5 Dec 2018 14:57:17 +0100 Subject: [PATCH 05/38] Update sketch_landscape.ino adjust the logic relation --- sketch_landscape.ino | 35 ++++++++++++++++++++++------------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/sketch_landscape.ino b/sketch_landscape.ino index 4824099..2507b14 100644 --- a/sketch_landscape.ino +++ b/sketch_landscape.ino @@ -225,20 +225,27 @@ int landscape(){ // also for 'T' and '十' //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, //rightandstraight - if(online_judge){ + if(online_judge()){ delay(1000); // experiment to determine this - if(onlyleft_judge) return 1; - else return 5; - - if(onlyright_judge) return 2; - else return 6; - - if(T_judge) return 3; - else return 4; - } + //这里的逻辑需要重新考虑一下，有坑能出现false全部return到第一个里面 + if(left_judge()){ + if (wal_judge()) return 1; + else return 5; + } + + if(right_judge() { + if (wall_judge()) return 1; + else return 2; + } + if(T_judge()){ + if (wall_judge()) return 3; + else return 4; + } + } } +//sensorValues是全局变量，不用传值就可调用 bool sensor_judge(num, range, erro){ // experiment to determine if sensorValues[0] is exact the 1st sensor!!! if (sensorValues[num] < range + erro && sensorValues[num] > range - erro) return true; @@ -263,11 +270,12 @@ bool online_judge(){ return false; } } + // there's no false, then return true return true; } } -bool onlyleft_judge(){ +bool left_judge(){ // tell onlyleft from 'left and straight' // the sensor values should be {1000, 1000, 1000, 1000, 0, 0} for (unsigned char i = 0;i < NUM_SENSORS; i++){ @@ -285,11 +293,12 @@ bool onlyleft_judge(){ return false; } } + // there's no false, then return true return true; } } -bool onlyright_judge(){ +bool right_judge(){ // tell onlylright from 'right and straight' // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} for (unsigned char i = 0;i < NUM_SENSORS; i++){ @@ -336,5 +345,5 @@ bool T_judge(){ bool wall_judge(){ // use ultra_sound to judge if there is a wall in fornt - + // return true if there's a wall in the front } From ca788394b6be1f6350ef5b2642309663f00f2f7a Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Wed, 5 Dec 2018 15:06:15 +0100 Subject: [PATCH 06/38] Update sketch_landscape.ino this version is only for the test of the landscape, without motor control. --- sketch_landscape.ino | 60 +++----------------------------------------- 1 file changed, 3 insertions(+), 57 deletions(-) diff --git a/sketch_landscape.ino b/sketch_landscape.ino index 2507b14..fefe7b4 100644 --- a/sketch_landscape.ino +++ b/sketch_landscape.ino @@ -66,6 +66,8 @@ void loop() { // limit their speed to 90~180 set_motors(leftMotorSpeed, rightMotorSpeed); //set_motors(90, 90); // stop + + Serial.print(landscape()); } void set_motors(int left_motorspeed, int right_motorspeed) { @@ -79,35 +81,6 @@ void set_motors(int left_motorspeed, int right_motorspeed) { right_motor.write(right_motorspeed); } -void init_lifter(){ - lifter.write(180); - return; -} - -void lift(){ - lifter.write(35); - return; -} - -void unlift(){ - lifter.write(180); - return; -} - -void init_gripper(){ - gripper.write(0); - return; -} - -void open_gripper(){ - gripper.write(0); - return; -} - -void close_gripper(){ - gripper.write(35); - return; -} void manual_calibration() { //calibrate for sometime by sliding the sensors across the line, @@ -189,34 +162,7 @@ void check_u_turn(int position){ u_turn = check_reflectance_dead_end(position); // stage2: check for ultra-sound sensor (maybe a variable that has been set before) - return u_turn; // 可以返回函数？？？ -} - -void u_turn(){ - for (int i=0; i<22500;i++){ - motor1.write(0); - motor2.write(0); - } - //set_motors(90, 90); - return; -} - -void right_turn(){ - for (int i=0; i<22500/2;i++){ - motor1.write(0); // 左轮向前，右轮倒转，是为右转 - motor2.write(180); - } - //set_motors(90, 90); - return; -} - -void left_turn(){ - for (int i=0; i<22500/2;i++){ - motor1.write(180); // 左轮向后，右轮向前，是为左转 - motor2.write(0); - } - //set_motors(90, 90); - return; + return 7; // 可以返回函数？？？ } int landscape(){ From 174a698773b75cb06098f408ca45f843d792732f Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Thu, 6 Dec 2018 14:29:23 +0100 Subject: [PATCH 07/38] Only for the use of test landscape judge MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.先测试sketch_nova26确定好relfectance sensor连接的顺序，是不是Arduino 3—8 连接 传感器1-6？ 2.sensorValues0—5是不是传感器1-6？影响到判断左右的函数 3.先测试直行函数，确定后轮是否能用？ 4.记录面包板上如何连线？ --- sketch_landscape.ino | 113 +++++-------------------------------------- 1 file changed, 13 insertions(+), 100 deletions(-) diff --git a/sketch_landscape.ino b/sketch_landscape.ino index fefe7b4..b1bbdba 100644 --- a/sketch_landscape.ino +++ b/sketch_landscape.ino @@ -2,14 +2,16 @@ 容纳graspping system的控制函数和line following的控制函数 */ #include +#include #include // Pololu QTR Sensor Library. Servo left_motor, right_motor; Servo gripper, lifter; -const int lifterPin = 9; -const int gripperPin = 10; +const int TrigPin = 9; +const int EchoPin = 10; const int left_motorPin = 11; const int right_motorPin = 12; +UltraSonicDistanceSensor distanceSensor(TrigPin, EchoPin); #define KP 0.2 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed #define KD 0.5 //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd) @@ -24,7 +26,7 @@ const int right_motorPin = 12; #define DEBUG 0 // 设置为1后，方便后面degbug时要不要打印出来传感器等的参数 // sensors are connected to digital pins 2 to 7 -QTRSensorsRC qtrrc((unsigned char[]) {2, 3, 4, 5, 6, 7}, +QTRSensorsRC qtrrc((unsigned char[]) { 3, 4, 5, 6, 7, 8}, // 3号传感器对应最左边or最右边要测清楚？ NUM_SENSORS, TIMEOUT, EMITTER_PIN); unsigned int sensorValues[NUM_SENSORS]; @@ -32,56 +34,14 @@ int lastError = 0; void setup() { left_motor.attach(left_motorPin); - right_motor.attach(right_motorPin); - gripper.attach(gripperPin); - lifter.attach(lifterPin); - set_motors(90, 90); // set the motors speed zero. - init_gripper(); - init_lifter(); + right_motor.attach(right_motorPin);. manual_calibration(); } void loop() { - int position = qtrrc.readLine(sensorValues); - //get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position. - - if (DEBUG) print_sensor_values(position); - - int error = position - 2500; - - if (DEBUG) print_error(error); - - int motorSpeed = KP * error + KD * (error - lastError); - - if (DEBUG) print_motorSpeed(motorSpeed); - - lastError = error; - - int leftMotorSpeed = 180 - left_motor_default_speed - motorSpeed; // closer to 0, speed up - int rightMotorSpeed = right_motor_default_speed - motorSpeed; // closer to 180, speed up, so slow down here - - if (DEBUG) print_left_right_motorSpeed(leftMotorSpeed, rightMotorSpeed); - - // set motor speeds using the two motor speed variables above - // limit their speed to 90~180 - set_motors(leftMotorSpeed, rightMotorSpeed); - //set_motors(90, 90); // stop - Serial.print(landscape()); } -void set_motors(int left_motorspeed, int right_motorspeed) { - // limit motor's speed to 0~90, 90~180 - if (180 - left_motorspeed > left_motor_max_speed ) left_motorspeed = 180 - left_motor_max_speed; // if speed is negative, use 0 instead - if (right_motorspeed > right_motor_max_speed ) right_motorspeed = right_motor_max_speed; - if ( left_motorspeed > 90) left_motorspeed = 90; // avoid spinning backward - if (right_motorspeed < 90) right_motorspeed = 90; - //left_motorspeed = 180 - left_motorspeed; - left_motor.write(left_motorspeed); - right_motor.write(right_motorspeed); -} - - void manual_calibration() { //calibrate for sometime by sliding the sensors across the line, //or you may use auto-calibration instead @@ -112,59 +72,6 @@ void manual_calibration() { // delay(1000); } -void print_sensor_values(int position){ - // for the use of calibration and debug - // print the sensor values as numbers from 0 to 1000, where 0 means maximum reflectance and - // 1000 means minimum reflectance, followed by the line position - for (unsigned char i = 0; i < NUM_SENSORS; i++){ - Serial.print(sensorValues[i]); - Serial.print('\t'); - } - //Serial.println(); // uncomment this line if you are using raw values - String pos = "Position: "; - String fpos = pos + position; - Serial.println(position); // comment this line out if you are using raw values - return; -} - -void print_error(int error){ - // for the use of debug - String err = "Error: "; - String ferr = err + error; - Serial.println(ferr); - Serial.println(); - return; -} - -void print_motorSpeed(int motorSpeed){ - Serial.println(motorSpeed); - return; -} - -void print_left_right_motorSpeed(int leftMotorSpeed, int rightMotorSpeed){ - Serial.println(leftMotorSpeed); - Serial.println(rightMotorSpeed); - return; -} - -bool check_reflectance_dead_end(int position){ - int lower_bound = 2300; - int higher_bound = 2700; - if (position > 2300 and position < 2700){ - return true; - } - return false; -} - -void check_u_turn(int position){ - u_turn = false; // we assume we shouldn't do u-turn - // stage1: check for reflectance bounds in a fixed range (determined by testing) - u_turn = check_reflectance_dead_end(position); - // stage2: check for ultra-sound sensor (maybe a variable that has been set before) - - return 7; // 可以返回函数？？？ -} - int landscape(){ // first make sure the car is online, then judge the landscape // use ultra sound sensors to distinguish 'only right' and 'right or straight' @@ -172,7 +79,7 @@ int landscape(){ //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, //rightandstraight if(online_judge()){ - delay(1000); // experiment to determine this + delay(1000); // experiment to determine this，实际中这里要不要delay需要商量 //这里的逻辑需要重新考虑一下，有坑能出现false全部return到第一个里面 if(left_judge()){ if (wal_judge()) return 1; @@ -292,4 +199,10 @@ bool T_judge(){ bool wall_judge(){ // use ultra_sound to judge if there is a wall in fornt // return true if there's a wall in the front + double distance = distanceSensor.measureDistanceCm(); + if(distance<20) + return true; + else + return false; + delay(200); } From 6f1153da0520f00fbeec1468a2f6b15f482f3d55 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Thu, 6 Dec 2018 14:34:26 +0100 Subject: [PATCH 08/38] Update README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 4f840f9..00961f0 100644 --- a/README.md +++ b/README.md @@ -1 +1,7 @@ # arduino-maze-robot +面包板上连线记录！！连好之后勿动，节约每次测试前的时间！！！ +1.传感器的供电全部使用Arduino5v +2.Motor， graspper lifter的供电直接使用充电宝； +4.面包板一侧的负极一条公共地，正极一条全部是电动机的5v供电； +6.LED校准状态指示灯直接连Arduino的pin13 +7.MOtor的pin11左 pin12右 From e71a489c7ae9ecabf810f9044c66915f17f5703a Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Thu, 6 Dec 2018 14:56:40 +0100 Subject: [PATCH 09/38] Update README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 00961f0..3b1f83c 100644 --- a/README.md +++ b/README.md @@ -5,3 +5,4 @@ 4.面包板一侧的负极一条公共地，正极一条全部是电动机的5v供电； 6.LED校准状态指示灯直接连Arduino的pin13 7.MOtor的pin11左 pin12右 +8.Reflectance sensor的1-6对应Arduino的pin3-8（从左到右是从绿到黑线） From a001bc1bc8779b7271828e0f79cff38125ca92b9 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Sun, 9 Dec 2018 17:30:25 +0100 Subject: [PATCH 10/38] Update sketch_landscape.ino MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 代码好看不如代码好用，不用for循环了； 在干净无灰的跑道上传感器的值稳定在500-600附近，online_judge满足需求； 如果还是不能用，就写一个找最大两个值的函数，找最值判断下标是不是要的下标。 --- sketch_landscape.ino | 175 ++++++++++++++++++------------------------- 1 file changed, 73 insertions(+), 102 deletions(-) diff --git a/sketch_landscape.ino b/sketch_landscape.ino index b1bbdba..14016a3 100644 --- a/sketch_landscape.ino +++ b/sketch_landscape.ino @@ -1,16 +1,16 @@ /* 容纳graspping system的控制函数和line following的控制函数 */ -#include +//#include #include #include // Pololu QTR Sensor Library. -Servo left_motor, right_motor; -Servo gripper, lifter; +//Servo left_motor, right_motor; +//Servo gripper, lifter; const int TrigPin = 9; const int EchoPin = 10; -const int left_motorPin = 11; -const int right_motorPin = 12; +//const int left_motorPin = 11; +//const int right_motorPin = 12; UltraSonicDistanceSensor distanceSensor(TrigPin, EchoPin); #define KP 0.2 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed @@ -33,13 +33,17 @@ unsigned int sensorValues[NUM_SENSORS]; int lastError = 0; void setup() { - left_motor.attach(left_motorPin); - right_motor.attach(right_motorPin);. + Serial.begin(9600); // 波特率 +// left_motor.attach(left_motorPin); +// right_motor.attach(right_motorPin); manual_calibration(); + } void loop() { - Serial.print(landscape()); + double distance = distanceSensor.measureDistanceCm(); + delay(3000); + landscape(); } void manual_calibration() { @@ -54,155 +58,122 @@ void manual_calibration() { qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call) } digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration - // print the calibration minimum values measured when emitters were on - Serial.begin(9600); for (int i = 0; i < NUM_SENSORS; i++){ Serial.print(qtrrc.calibratedMinimumOn[i]); Serial.print(' '); } Serial.println(); - // print the calibration maximum values measured when emitters were on for (int i = 0; i < NUM_SENSORS; i++){ Serial.print(qtrrc.calibratedMaximumOn[i]); Serial.print(' '); } Serial.println(); - // delay(1000); } + int landscape(){ // first make sure the car is online, then judge the landscape // use ultra sound sensors to distinguish 'only right' and 'right or straight' // also for 'T' and '十' //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, //rightandstraight + int position = qtrrc.readLine(sensorValues); + + Serial.print(left_judge()); + Serial.print('\n'); + /* if(online_judge()){ - delay(1000); // experiment to determine this，实际中这里要不要delay需要商量 - //这里的逻辑需要重新考虑一下，有坑能出现false全部return到第一个里面 + // 实际中需要往前跑一段再判断 if(left_judge()){ - if (wal_judge()) return 1; + if (wall_judge()) return 1; // use true instead of wall_judge() else return 5; } - if(right_judge() { - if (wall_judge()) return 1; - else return 2; + if(wall_judge()) { + if (true) return 2; + else return 6; } - if(T_judge()){ - if (wall_judge()) return 3; + if(wall_judge()){ + if (true) return 3; else return 4; } - } + }*/ } -//sensorValues是全局变量，不用传值就可调用 -bool sensor_judge(num, range, erro){ - // experiment to determine if sensorValues[0] is exact the 1st sensor!!! - if (sensorValues[num] < range + erro && sensorValues[num] > range - erro) return true; - else return false; + +bool sensor_judge(unsigned int num, int range, int error){ + int a = sensorValues[num]; + if ((a <= range + error) && (a >= range - error)) return true; + else return false; } bool online_judge(){ // first judge if the raw vaues array is 0 0 1000 1000 0 0 // erro is ±250, change it after experiment - for (unsigned char i = 0;i < NUM_SENSORS; i++){ - if (i != 2 or 3){ - unsigned int range = 0; - unsigned int erro = 250; - if (sensor_judge(i, range, erro) == false) { - return false; - } - } - else{ - unsigned int range = 1000; - unsigned int erro = 250; - if (sensor_judge(i, range, erro) == false){ - return false; - } - } - // there's no false, then return true - return true; - } + for (unsigned int i = 0;i < NUM_SENSORS; i++){ + Serial.print(i); + Serial.print(' '); + Serial.print(sensorValues[i]); + Serial.print('\t'); + } + bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); + return a && b; } -bool left_judge(){ + +boolean left_judge(){ // tell onlyleft from 'left and straight' // the sensor values should be {1000, 1000, 1000, 1000, 0, 0} - for (unsigned char i = 0;i < NUM_SENSORS; i++){ - if (i != 4 or 5){ - unsigned int range = 1000; - unsigned int erro = 250; - if (sensor_judge(i, range, erro) == false) { - return false; - } - } - else{ - unsigned int range = 0; - unsigned int erro = 250; - if (sensor_judge(i, range, erro) == false){ - return false; - } - } - // there's no false, then return true - return true; - } + for (unsigned int i = 0;i < NUM_SENSORS; i++){ + Serial.print(i); + Serial.print(' '); + Serial.print(sensorValues[i]); + Serial.print('\t'); + } + bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); } bool right_judge(){ + for (unsigned int i = 0;i < NUM_SENSORS; i++){ + Serial.print(i); + Serial.print(' '); + Serial.print(sensorValues[i]); + Serial.print('\t'); + } // tell onlylright from 'right and straight' // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} - for (unsigned char i = 0;i < NUM_SENSORS; i++){ - if (i != 1 or 2 ){ - unsigned int range = 10000; - unsigned int erro = 250; - if (sensor_judge(i, range, erro) == false) { - return false; - } - } - else{ - unsigned int range = 0; - unsigned int erro = 250; - if (sensor_judge(i, range, erro) == false){ - return false; - } - } - return true; - } + bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 350, 650) && sensor_judge(5, 650, 350); + return a && b; } bool T_judge(){ // tell T from 十 // the sensor values should be all 1000 - for (unsigned char i = 0;i < NUM_SENSORS; i++){ - if (i != 1 or 2 ){ - unsigned int range = 10000; - unsigned int erro = 250; - if (sensor_judge(i, range, erro) == false) { - return false; - } - } - else{ - unsigned int range = 0; - unsigned int erro = 250; - if (sensor_judge(i, range, erro) == false){ - return false; - } - } - return true; - } + for (unsigned int i = 0;i < NUM_SENSORS; i++){ + Serial.print(i); + Serial.print(' '); + Serial.print(sensorValues[i]); + Serial.print('\t'); + } + bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); + return a && b; } -bool wall_judge(){ +bool wall_judge(double distance){ // use ultra_sound to judge if there is a wall in fornt // return true if there's a wall in the front - double distance = distanceSensor.measureDistanceCm(); - if(distance<20) - return true; + + if(distance<10) + return 1; else - return false; + return 0; delay(200); } From d72f462f76081806c716f02fe88cc2305bd8aed7 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Mon, 10 Dec 2018 17:41:15 +0100 Subject: [PATCH 11/38] Create PID_landscape --- PID_landscape | 242 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 242 insertions(+) create mode 100644 PID_landscape diff --git a/PID_landscape b/PID_landscape new file mode 100644 index 0000000..449071e --- /dev/null +++ b/PID_landscape @@ -0,0 +1,242 @@ +#include +#include +#include // Pololu QTR Sensor Library. + +//问题在于PID需要reflectance sensor的1-6连到Arduino的8-3， +//要把插线顺序调过来i，不仅要改error的正负，还要改kp和kd大小，因为插线正反影响每个传感器的权值 +//如果要改landscape，整个左右p判断要反过来 + +Servo left_motor, right_motor; +//Servo gripper, lifter; +const int TrigPin = 9; +const int EchoPin = 10; +const int left_motorPin = 11; +const int right_motorPin = 12; +UltraSonicDistanceSensor distanceSensor(TrigPin, EchoPin); +double distance; + +#define KP 0.2 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed +#define KD 0.5 //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd) +#define left_motor_default_speed 180 //normal speed of the left_motor +#define right_motor_default_speed 180 //normal speed of the right_motor +#define left_motor_max_speed 180 //max. speed of the left_motor +#define right_motor_max_speed 180 //max. speed of the right_motor + +#define NUM_SENSORS 6 //number of sensors used +#define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low +#define EMITTER_PIN 8 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 + + +// sensors are connected to digital pins 2 to 7 +QTRSensorsRC qtrrc((unsigned char[]) { 3, 4, 5, 6, 7, 8}, // 3号传感器对应最左边or最右边要测清楚？ + NUM_SENSORS, TIMEOUT, EMITTER_PIN); + +unsigned int sensorValues[NUM_SENSORS]; +int lastError = 0; + +void setup() { + Serial.begin(9600); // 波特率 + left_motor.attach(left_motorPin); + right_motor.attach(right_motorPin); + set_motors(90,90); // set the motors not moving + delay(1500); + manual_calibration(); +} + +void loop() { + distance = distanceSensor.measureDistanceCm(); + + /*delay(3000); + Serial.print(landscape()); + Serial.print('\n');*/ + //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, + //rightandstraight + int type = landscape(); + + /* + switch(type){ + case 1:break; // 左右弯交给PID处理 + case 2:break; + case 3: right_turn(); break; + case 5: go_straight(); break; + case 6: go_straight(); break; + case 7: break; // 不考虑 U turn的话，应该是直线，交给PID + }*/ + int position = qtrrc.readLine(sensorValues); + //int error = -2500 + position; + int error = 2500 - position; //换掉左右不仅改变了正负，还改变了每个传感器的权值，要么改Kp， Kd,要么需要把后面左右判断全改了 + int motorSpeed = KP * error + KD * (error - lastError); + //Serial.println(motorSpeed); + + lastError = error; + + int leftMotorSpeed = 180 - (left_motor_default_speed + motorSpeed); // closer to 0, speed up + int rightMotorSpeed = right_motor_default_speed - motorSpeed; // closer to 180, speed up, so slow down here + + set_motors(leftMotorSpeed, rightMotorSpeed); +} + +void set_motors(int left_motorspeed, int right_motorspeed) { + // limit motor's speed to 0~90, 90~180 + if (180 - left_motorspeed > left_motor_max_speed ) left_motorspeed = 180 - left_motor_max_speed; // if speed is negative, use 0 instead + if (right_motorspeed > right_motor_max_speed ) right_motorspeed = right_motor_max_speed; + if ( left_motorspeed > 90) left_motorspeed = 90; // avoid spinning backward + if (right_motorspeed < 90) right_motorspeed = 90; + //left_motorspeed = 180 - left_motorspeed; + left_motor.write(left_motorspeed); + right_motor.write(right_motorspeed); +} + + +void manual_calibration() { + //calibrate for sometime by sliding the sensors across the line, + //or you may use auto-calibration instead + + pinMode(13, OUTPUT); + digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode + //void emittersOn(); + for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds + { + qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call) + } + digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration + // print the calibration minimum values measured when emitters were on + for (int i = 0; i < NUM_SENSORS; i++){ + Serial.print(qtrrc.calibratedMinimumOn[i]); + Serial.print(' '); + } + Serial.println(); + // print the calibration maximum values measured when emitters were on + for (int i = 0; i < NUM_SENSORS; i++){ + Serial.print(qtrrc.calibratedMaximumOn[i]); + Serial.print(' '); + } + Serial.println(); +} + + +int landscape(){ + // first make sure the car is online, then judge the landscape + // use ultra sound sensors to distinguish 'only right' and 'right or straight' + // also for 'T' and '十' + //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, + //rightandstraight + int position = qtrrc.readLine(sensorValues); + /* // for debugging + for (unsigned int i = 0;i < NUM_SENSORS; i++){ + Serial.print(i); + Serial.print(' '); + Serial.print(sensorValues[i]); + Serial.print('\t'); + }*/ + + // 实际中需要往前跑一段再判断 + Serial.print(12); + Serial.print(' '); + //delay(9000); //loop里面一个循环，传感器更新一次值，所以还在用上一次的值 + if(left_judge()){ + if (wall_judge()) return 1; // use true instead of wall_judge() + else return 5; + } + + if(right_judge()) { + if (wall_judge()) return 2; + else return 6; + } + + if(T_judge()){ + if (wall_judge()) return 3; + else return 4; + } + return 7; // 用于调试 +} + + +bool sensor_judge(unsigned int num, int range, int error){ + int a = sensorValues[num]; + if ((a <= range + error) && (a >= range - error)) return true; + else return false; + } + +boolean right_judge(){ + // tell onlyleft from 'left and straight' + // the sensor values should be {1000, 1000, 1000, 1000, 0, 0} + bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); + return a && b; + } + +bool left_judge(){ + // tell onlylright from 'right and straight' + // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} + bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 350, 650) && sensor_judge(5, 650, 350); + return a && b; + } + +bool T_judge(){ + // tell T from 十 + // the sensor values should be all 1000 + bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); + return a && b; + } + + +bool wall_judge(){ + // use ultra_sound to judge if there is a wall in fornt + // return true if there's a wall in the front + if((distance>10) && (distance<20)) + return true; + else + return false; + } + + +void right_turn(){ + left_motor.write(0); //左轮正转 + right_motor.write(90); //右轮反转 + delay(2150/2); + return; +} + +void left_turn(){ + left_motor.write(90); //左轮正转 + right_motor.write(180); //右轮反转 + delay(2150/2); + return; +} + +void go_straight(){ + left_motor.write(0); //左轮正转 + right_motor.write(180); //右轮反转 + delay(1000); + return; + } + + +/* +bool check_reflectance_dead_end(int position){ + int lower_bound = 2300; + int higher_bound = 2700; + if (position > 2300 and position < 2700){ + return true; + } + return false; +} + +void check_u_turn(int position){ + u_turn = false; // we assume we shouldn't do u-turn + // stage1: check for reflectance bounds in a fixed range (determined by testing) + u_turn = check_reflectance_dead_end(position); + // stage2: check for ultra-sound sensor (maybe a variable that has been set before) + + return u_turn; +} + +void u_turn(){ + motor1.write(0); //左轮正转 + motor2.write(0); //右轮反转 + delay(2150); + return; +}*/ From 64a91289ea3ed02fba6047cb12885696be14574e Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Mon, 10 Dec 2018 17:56:17 +0100 Subject: [PATCH 12/38] Update README.md --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 3b1f83c..6339307 100644 --- a/README.md +++ b/README.md @@ -6,3 +6,6 @@ 6.LED校准状态指示灯直接连Arduino的pin13 7.MOtor的pin11左 pin12右 8.Reflectance sensor的1-6对应Arduino的pin3-8（从左到右是从绿到黑线） + + +现在的问题是PID到底怎么连线才能正确寻线？ From 8c05f55a812996100e0662da8d8158a930766ce1 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 12:06:44 +0100 Subject: [PATCH 13/38] Update sketch_landscape.ino --- sketch_landscape.ino | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sketch_landscape.ino b/sketch_landscape.ino index 14016a3..4c80c49 100644 --- a/sketch_landscape.ino +++ b/sketch_landscape.ino @@ -1,5 +1,6 @@ /* - 容纳graspping system的控制函数和line following的控制函数 + 这版基于reflectance sensor的1-6引脚对应Arduino的3-8引脚，测试通过 + This version is based on reflectance sensor's pin 1-6 coonected to Arduino's digital pin 3-8 */ //#include #include From e214ddcffe4c196f2e88af394519f4071ac0c8ef Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 12:08:04 +0100 Subject: [PATCH 14/38] Only PID --- sketch_nov26a.ino | 135 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 135 insertions(+) create mode 100644 sketch_nov26a.ino diff --git a/sketch_nov26a.ino b/sketch_nov26a.ino new file mode 100644 index 0000000..a1bf1a0 --- /dev/null +++ b/sketch_nov26a.ino @@ -0,0 +1,135 @@ +#include +#include +//这版代码需要把Reflectance Sesors的1~6连到Arduino的8~3，才能正常工作 + +Servo motor1, motor2; +const int motor1Pin = 11; //left +const int motor2Pin = 12; +//AF_DCMotor motor1(1, MOTOR12_1KHZ ); //create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency +//AF_DCMotor motor2(2, MOTOR12_1KHZ ); //create motor #2 using M2 output on Motor Drive Shield, set to 1kHz PWM frequency + +#define KP 0.2 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed +#define KD 0.5 //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd) +#define M1_default_speed 180 //normal speed of the Motor1， 不够快！！ +#define M2_default_speed 180 //normal speed of the Motor2 +#define M1_maksimum_speed 180 //max. speed of the Motor1 +#define M2_maksimum_speed 180 //max. speed of the Motor2 +//#define MIDDLE_SENSOR 2 //number of middle sensor used + +#define NUM_SENSORS 6 //number of sensors used +#define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low +#define EMITTER_PIN 7 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 +#define DEBUG 1 + +//sensors 0 through 5 are connected to analog inputs 0 through 5, respectively +QTRSensorsRC qtrrc((unsigned char[]) {3, 4, 5, 6, 7, 8}, + NUM_SENSORS, TIMEOUT, EMITTER_PIN); + +unsigned int sensorValues[NUM_SENSORS]; + +void setup(){ + motor1.attach(motor1Pin); + motor2.attach(motor2Pin); + set_motors(90,90); // set the motors not moving + delay(1500); + manual_calibration(); +} + +int lastError = 0; +int last_proportional = 0; +int integral = 0; + +void loop(){ + //unsigned int sensors[2]; + int position = qtrrc.readLine(sensorValues); //get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position. + + // write a function to change the position, espeacially for crooss and T corner + //测试中能把传感器都打出来，传感器的固定 + // print the sensor values as numbers from 0 to 1000, where 0 means maximum reflectance and + // 1000 means minimum reflectance, followed by the line position + for (unsigned char i = 0; i < NUM_SENSORS; i++) + { + Serial.print(sensorValues[i]); + Serial.print('\t'); + } + //Serial.println(); // uncomment this line if you are using raw values*/ + String pos = "Position: "; + String fpos = pos + position; + Serial.println(fpos); // comment this line out if you are using raw values + + int error = -2500 + position; + + /* + String err = "Error: "; + String ferr = err + error; + Serial.println(ferr); + Serial.println();*/ + + int motorSpeed = KP * error + KD * (error - lastError); + //Serial.println(motorSpeed); + + lastError = error; + + int leftMotorSpeed = 180 - (M1_default_speed + motorSpeed); // closer to 0, speed up + int rightMotorSpeed = M2_default_speed - motorSpeed; // closer to 180, speed up, so slow down here + //Serial.println(leftMotorSpeed); + //Serial.println(rightMotorSpeed); + + //delay(10); + + // set motor speeds using the two motor speed variables above + // limit their speed to 90~180 + set_motors(leftMotorSpeed, rightMotorSpeed); + //set_motors(90, 90); + //set_motors(0, 180); +} + + + +void set_motors(int motor1speed, int motor2speed){ + if (180 - motor1speed >= M1_maksimum_speed ) motor1speed = 180 - M1_maksimum_speed; // if speed is negative, use 0 instrad + if (motor2speed >= M2_maksimum_speed ) motor2speed = M2_maksimum_speed; + if ( motor1speed >= 90) motor1speed = 90; // avoid spinning backward + if (motor2speed <= 90) motor2speed = 90; + motor1.write(motor1speed); + motor2.write(motor2speed); +} + + + + +void manual_calibration() { + //calibrate for sometime by sliding the sensors across the line, + //or you may use auto-calibration instead + pinMode(2, OUTPUT); + digitalWrite(2, LOW); + delay(500); + + pinMode(13, OUTPUT); + digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode + //void emittersOn(); + for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds + { + qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call) + } + digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration + + // print the calibration minimum values measured when emitters were on + Serial.begin(9600); + for (int i = 0; i < NUM_SENSORS; i++) + { + Serial.print(qtrrc.calibratedMinimumOn[i]); + Serial.print(' '); + } + Serial.println(); + + // print the calibration maximum values measured when emitters were on + for (int i = 0; i < NUM_SENSORS; i++) + { + Serial.print(qtrrc.calibratedMaximumOn[i]); + Serial.print(' '); + } + Serial.println(); + Serial.println(); + delay(1000); +} From 149449bc8c8850900148086a3d48f4b66f8be48d Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 12:19:04 +0100 Subject: [PATCH 15/38] Update README.md --- README.md | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 6339307..865ad5c 100644 --- a/README.md +++ b/README.md @@ -8,4 +8,24 @@ 8.Reflectance sensor的1-6对应Arduino的pin3-8（从左到右是从绿到黑线） -现在的问题是PID到底怎么连线才能正确寻线？ +PID算法工作是基于sensor的1-6， 接Arduino的8-3; +Arduino的3-8一定对应屏幕上的sensorValues0-5; + +landscape算法基于sensor的1-6， 接Arduino的3-8； +将两者合并，最简单的办法是该landscape的判断条件。 + +车头向前，从左到右： +sensor pin 6 5 4 3 2 1 +Arduino pin 3 4 5 6 7 8 +sensorValues 0 1 2 3 4 5 + +eg. 遇到左转弯，左边黑，右边白， 这样空间的左右和sensorvalues的从左到右是对应的 +sensor pin 6 5 4 3 2 1 + 1 1 1 1 0 0 +Arduino pin 3 4 5 6 7 8 + +sensorValues 0 1 2 3 4 5 + +T不变，左右转要对调即可 + + From 73d1ece557811817da8d81a4628b0574f8903f28 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 18:01:42 +0100 Subject: [PATCH 16/38] Update README.md --- README.md | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 865ad5c..7fe945e 100644 --- a/README.md +++ b/README.md @@ -15,9 +15,10 @@ landscape算法基于sensor的1-6， 接Arduino的3-8； 将两者合并，最简单的办法是该landscape的判断条件。 车头向前，从左到右： -sensor pin 6 5 4 3 2 1 -Arduino pin 3 4 5 6 7 8 -sensorValues 0 1 2 3 4 5 +sensor pin|6| 5| 4| 3| 2| 1 +---|---|---|---|--- +Arduino pin| 3| 4| 5| 6| 7| 8 +sensorValues| 0| 1| 2| 3| 4| 5 eg. 遇到左转弯，左边黑，右边白， 这样空间的左右和sensorvalues的从左到右是对应的 sensor pin 6 5 4 3 2 1 @@ -29,3 +30,11 @@ sensorValues 0 1 2 3 4 5 T不变，左右转要对调即可 +//更改PID参数 +目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯 +转弯半径大的原因是传感器的位置不够靠前，解决办法只有把传感器向前固定 + +其实目前已经用了最大速度，所以更改Kp， Kd值没有效果，转弯的时候一个轮停住，另一个动，是自然的结果 + +速度调小到135效果更差 + From 50f5aa46bedabb5527dc7f87bb845386a29c3de3 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 18:11:57 +0100 Subject: [PATCH 17/38] Update README.md --- README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 7fe945e..f20a6d2 100644 --- a/README.md +++ b/README.md @@ -1,18 +1,18 @@ # arduino-maze-robot -面包板上连线记录！！连好之后勿动，节约每次测试前的时间！！！ -1.传感器的供电全部使用Arduino5v -2.Motor， graspper lifter的供电直接使用充电宝； -4.面包板一侧的负极一条公共地，正极一条全部是电动机的5v供电； -6.LED校准状态指示灯直接连Arduino的pin13 -7.MOtor的pin11左 pin12右 -8.Reflectance sensor的1-6对应Arduino的pin3-8（从左到右是从绿到黑线） +**面包板上连线记录！！连好之后勿动，节约每次测试前的时间！！！** +* 传感器的供电全部使用Arduino 5v； +* Motor， graspper lifter的供电直接使用充电宝； +* 面包板一侧的负极一条公共地，正极一条全部是电动机的5v供电； +* LED校准状态指示灯直接连Arduino的pin13； +* MOtor的pin11左 pin12右； +* Reflectance sensor的1-6对应Arduino的pin8-3，保证传感器（小车）的左右和屏幕上的左右是一致的。 PID算法工作是基于sensor的1-6， 接Arduino的8-3; Arduino的3-8一定对应屏幕上的sensorValues0-5; landscape算法基于sensor的1-6， 接Arduino的3-8； -将两者合并，最简单的办法是该landscape的判断条件。 +将两者合并，最简单的办法是更改landscape的判断条件。 车头向前，从左到右： sensor pin|6| 5| 4| 3| 2| 1 From bf821202cea8c49615b3879db4189a300bf9bf03 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 18:17:21 +0100 Subject: [PATCH 18/38] Update README.md --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index f20a6d2..59d47bc 100644 --- a/README.md +++ b/README.md @@ -15,10 +15,10 @@ landscape算法基于sensor的1-6， 接Arduino的3-8； 将两者合并，最简单的办法是更改landscape的判断条件。 车头向前，从左到右： -sensor pin|6| 5| 4| 3| 2| 1 ----|---|---|---|--- -Arduino pin| 3| 4| 5| 6| 7| 8 -sensorValues| 0| 1| 2| 3| 4| 5 +sensor pin| 6 | 5 | 4 | 3 | 2 | 1 +--- | --- | --- | --- | --- +Arduino pin| 3 | 4 | 5 | 6 | 7 | 8 +sensorValues| 0| 1 | 2 | 3 | 4 | 5 eg. 遇到左转弯，左边黑，右边白， 这样空间的左右和sensorvalues的从左到右是对应的 sensor pin 6 5 4 3 2 1 From 268cb6c24c0aafa25d591f19ca0e75ad2e8060c6 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 18:18:56 +0100 Subject: [PATCH 19/38] Update README.md --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 59d47bc..19510bc 100644 --- a/README.md +++ b/README.md @@ -15,10 +15,10 @@ landscape算法基于sensor的1-6， 接Arduino的3-8； 将两者合并，最简单的办法是更改landscape的判断条件。 车头向前，从左到右： -sensor pin| 6 | 5 | 4 | 3 | 2 | 1 ---- | --- | --- | --- | --- -Arduino pin| 3 | 4 | 5 | 6 | 7 | 8 -sensorValues| 0| 1 | 2 | 3 | 4 | 5 + sensor pin| 6 | 5 | 4 | 3 | 2 | 1 + --- | --- | --- | --- | --- + Arduino pin| 3 | 4 | 5 | 6 | 7 | 8 + sensorValues| 0 | 1 | 2 | 3 | 4 | 5 eg. 遇到左转弯，左边黑，右边白， 这样空间的左右和sensorvalues的从左到右是对应的 sensor pin 6 5 4 3 2 1 From a0ae077c7698b4ecbf44fe662c372e0f3d0c8711 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 18:19:18 +0100 Subject: [PATCH 20/38] Update README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 19510bc..f527410 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,7 @@ landscape算法基于sensor的1-6， 接Arduino的3-8； 将两者合并，最简单的办法是更改landscape的判断条件。 车头向前，从左到右： + sensor pin| 6 | 5 | 4 | 3 | 2 | 1 --- | --- | --- | --- | --- Arduino pin| 3 | 4 | 5 | 6 | 7 | 8 From 78ed1d05e626dad92bb2ebb95de90379bab75950 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 22:05:45 +0100 Subject: [PATCH 21/38] Update README.md --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index f527410..a625022 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,11 @@ landscape算法基于sensor的1-6， 接Arduino的3-8； 车头向前，从左到右： + +1|2|3 +--|--|-- +4|5|6 + sensor pin| 6 | 5 | 4 | 3 | 2 | 1 --- | --- | --- | --- | --- Arduino pin| 3 | 4 | 5 | 6 | 7 | 8 From 3d7163a83d8b20d5773db48941e894083c560c76 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 22:07:46 +0100 Subject: [PATCH 22/38] Update README.md --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index a625022..edd6845 100644 --- a/README.md +++ b/README.md @@ -21,10 +21,10 @@ landscape算法基于sensor的1-6， 接Arduino的3-8； --|--|-- 4|5|6 - sensor pin| 6 | 5 | 4 | 3 | 2 | 1 - --- | --- | --- | --- | --- - Arduino pin| 3 | 4 | 5 | 6 | 7 | 8 - sensorValues| 0 | 1 | 2 | 3 | 4 | 5 +sensorpin|6|5|4|3|2|1 +---|---|---|---|--- +Arduinopin|3|4|5|6|7|8 +sensorValues|0|1|2|3|4|5 eg. 遇到左转弯，左边黑，右边白， 这样空间的左右和sensorvalues的从左到右是对应的 sensor pin 6 5 4 3 2 1 From d8f7cb057d01070aad1dd5e34f47c670f25dc90c Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 22:09:25 +0100 Subject: [PATCH 23/38] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index edd6845..fdd8018 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ landscape算法基于sensor的1-6， 接Arduino的3-8； 4|5|6 sensorpin|6|5|4|3|2|1 ----|---|---|---|--- +---|---|---|---|---|--- Arduinopin|3|4|5|6|7|8 sensorValues|0|1|2|3|4|5 From 2db2e5ba6e06d03bba3c5db8ac1dac4fcea2f74e Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 22:10:21 +0100 Subject: [PATCH 24/38] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index fdd8018..93ab691 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ landscape算法基于sensor的1-6， 接Arduino的3-8； 4|5|6 sensorpin|6|5|4|3|2|1 ----|---|---|---|---|--- +---|---|---|---|---|---|--- Arduinopin|3|4|5|6|7|8 sensorValues|0|1|2|3|4|5 From 0014f264f1424767b9ae05c68523f3176baec7eb Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 22:15:15 +0100 Subject: [PATCH 25/38] Update README.md --- README.md | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 93ab691..d2dba42 100644 --- a/README.md +++ b/README.md @@ -16,22 +16,17 @@ landscape算法基于sensor的1-6， 接Arduino的3-8； 车头向前，从左到右： - -1|2|3 ---|--|-- -4|5|6 - -sensorpin|6|5|4|3|2|1 +sensor pin|6|5|4|3|2|1 ---|---|---|---|---|---|--- -Arduinopin|3|4|5|6|7|8 +Arduino pin|3|4|5|6|7|8 sensorValues|0|1|2|3|4|5 eg. 遇到左转弯，左边黑，右边白， 这样空间的左右和sensorvalues的从左到右是对应的 -sensor pin 6 5 4 3 2 1 - 1 1 1 1 0 0 -Arduino pin 3 4 5 6 7 8 - + +sensor pin| 6| 5| 4| 3| 2| 1 +--|--|--|--|--|--|-- sensorValues 0 1 2 3 4 5 + |1| 1| 1| 1| 0| 0| T不变，左右转要对调即可 From ade66d4c45089be8dd992b60951fdba79bea5443 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 23:15:47 +0100 Subject: [PATCH 26/38] Update README.md --- README.md | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index d2dba42..1f04416 100644 --- a/README.md +++ b/README.md @@ -25,13 +25,13 @@ eg. 遇到左转弯，左边黑，右边白， 这样空间的左右和sensorval sensor pin| 6| 5| 4| 3| 2| 1 --|--|--|--|--|--|-- -sensorValues 0 1 2 3 4 5 - |1| 1| 1| 1| 0| 0| +[i]0 1 2 3 4 5 + sensorValuse[i]|1| 1| 1| 1| 0| 0| -T不变，左右转要对调即可 +结论：所以对landscape修改，T不变，左右转要对调即可。 -//更改PID参数 +更改PID参数 目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯 转弯半径大的原因是传感器的位置不够靠前，解决办法只有把传感器向前固定 @@ -39,3 +39,12 @@ T不变，左右转要对调即可 速度调小到135效果更差 + +关于dead end的两种方案： +1. 用1个ultra sound传感器，当reflectance sensors检测到全为0或者全1000的时候，检测距离，在范围内就可以判断为dead end； + * 原理：利用dead end和去掉线的地方到墙的距离不一样， ultra sound sensor可以得到距离的特点； + * 优点：用传感器少，耗时也少； + * 缺点：需要实验，会不会错过T， 导致误判？ +2. 用2个ultra sound传感器，分别装在前面和右边。当当reflectance sensors检测到全为0或者全1000的时候，判断是否前面和右边是否有墙。如果都有，左转，再判断，没有，则左转；有，则继续转90°，达到U-turn。 + * 优点：比方案一更稳妥； + * 缺点：需要两次检测、中间要停下来，更耗时。 From a17d4bc14edb2e0fa72f7f64dcd5f89a3d6eb3bb Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 23:21:38 +0100 Subject: [PATCH 27/38] Update README.md --- README.md | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 1f04416..6a310c9 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ # arduino-maze-robot +##记录连线 **面包板上连线记录！！连好之后勿动，节约每次测试前的时间！！！** * 传感器的供电全部使用Arduino 5v； * Motor， graspper lifter的供电直接使用充电宝； @@ -7,7 +8,8 @@ * MOtor的pin11左 pin12右； * Reflectance sensor的1-6对应Arduino的pin8-3，保证传感器（小车）的左右和屏幕上的左右是一致的。 - +------------------------------------------------------------------------------- +##解决PID和landscape不兼容的问题 PID算法工作是基于sensor的1-6， 接Arduino的8-3; Arduino的3-8一定对应屏幕上的sensorValues0-5; @@ -25,22 +27,26 @@ eg. 遇到左转弯，左边黑，右边白， 这样空间的左右和sensorval sensor pin| 6| 5| 4| 3| 2| 1 --|--|--|--|--|--|-- -[i]0 1 2 3 4 5 +[i]|0| 1| 2| 3| 4| 5 sensorValuse[i]|1| 1| 1| 1| 0| 0| -结论：所以对landscape修改，T不变，左右转要对调即可。 - +-[x]**结论**：所以对landscape修改，T不变，左右转要对调即可。 -更改PID参数 +----------------------------------------------------------------------------- +##改善转弯表现 目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯 -转弯半径大的原因是传感器的位置不够靠前，解决办法只有把传感器向前固定 +转弯半径大的原因是传感器的位置不够靠前，解决办法把传感器向前固定 + +速度调小到135效果更差 其实目前已经用了最大速度，所以更改Kp， Kd值没有效果，转弯的时候一个轮停住，另一个动，是自然的结果 -速度调小到135效果更差 +最终，取消90的速度限制，车轮可以在转弯时反转。 + + -关于dead end的两种方案： +##关于dead end的两种方案： 1. 用1个ultra sound传感器，当reflectance sensors检测到全为0或者全1000的时候，检测距离，在范围内就可以判断为dead end； * 原理：利用dead end和去掉线的地方到墙的距离不一样， ultra sound sensor可以得到距离的特点； * 优点：用传感器少，耗时也少； From 4091421f0c6d60573edc073b2e7f7635627ad15d Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 23:24:56 +0100 Subject: [PATCH 28/38] Update README.md --- README.md | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 6a310c9..181c070 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # arduino-maze-robot -##记录连线 +## 记录连线 **面包板上连线记录！！连好之后勿动，节约每次测试前的时间！！！** * 传感器的供电全部使用Arduino 5v； * Motor， graspper lifter的供电直接使用充电宝； @@ -33,20 +33,18 @@ sensor pin| 6| 5| 4| 3| 2| 1 -[x]**结论**：所以对landscape修改，T不变，左右转要对调即可。 ----------------------------------------------------------------------------- -##改善转弯表现 -目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯 -转弯半径大的原因是传感器的位置不够靠前，解决办法把传感器向前固定 +## 改善转弯表现 + 目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯转弯半径大的原因是传感器的位置不够靠前，解决办法：把传感器向前固定。 + 实验发现： + 1. 速度调小到135效果更差； + 2. Kp改到0.7， Kd改到0.1 无明显变化，可以尝试更大的倍数。 + 分析：其实目前已经用了最大速度，所以更改Kp， Kd值没有效果，转弯的时候一个轮停住，另一个动，是自然的结果 + 结果：最终，取消90的速度限制，车轮可以在转弯时反转。 -速度调小到135效果更差 -其实目前已经用了最大速度，所以更改Kp， Kd值没有效果，转弯的时候一个轮停住，另一个动，是自然的结果 -最终，取消90的速度限制，车轮可以在转弯时反转。 - - - -##关于dead end的两种方案： +## 关于dead end的两种方案： 1. 用1个ultra sound传感器，当reflectance sensors检测到全为0或者全1000的时候，检测距离，在范围内就可以判断为dead end； * 原理：利用dead end和去掉线的地方到墙的距离不一样， ultra sound sensor可以得到距离的特点； * 优点：用传感器少，耗时也少； From 398a45ef9d218672d7ea9c9c78550eaa35e84fa8 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 23:27:15 +0100 Subject: [PATCH 29/38] Update README.md --- README.md | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 181c070..77a3515 100644 --- a/README.md +++ b/README.md @@ -8,14 +8,15 @@ * MOtor的pin11左 pin12右； * Reflectance sensor的1-6对应Arduino的pin8-3，保证传感器（小车）的左右和屏幕上的左右是一致的。 -------------------------------------------------------------------------------- -##解决PID和landscape不兼容的问题 + +## 解决PID和landscape不兼容的问题 PID算法工作是基于sensor的1-6， 接Arduino的8-3; Arduino的3-8一定对应屏幕上的sensorValues0-5; landscape算法基于sensor的1-6， 接Arduino的3-8； 将两者合并，最简单的办法是更改landscape的判断条件。 +**引脚对应情况** 车头向前，从左到右： sensor pin|6|5|4|3|2|1 @@ -30,9 +31,9 @@ sensor pin| 6| 5| 4| 3| 2| 1 [i]|0| 1| 2| 3| 4| 5 sensorValuse[i]|1| 1| 1| 1| 0| 0| --[x]**结论**：所以对landscape修改，T不变，左右转要对调即可。 +**结论**：所以对landscape修改，T不变，左右转要对调即可。 + ------------------------------------------------------------------------------ ## 改善转弯表现 目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯转弯半径大的原因是传感器的位置不够靠前，解决办法：把传感器向前固定。 实验发现： From 0117101aaed0880eb092821b4cc3b00b89f786cd Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 23:28:36 +0100 Subject: [PATCH 30/38] Update README.md --- README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 77a3515..3b3fe5e 100644 --- a/README.md +++ b/README.md @@ -38,12 +38,12 @@ sensor pin| 6| 5| 4| 3| 2| 1 目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯转弯半径大的原因是传感器的位置不够靠前，解决办法：把传感器向前固定。 实验发现： 1. 速度调小到135效果更差； - 2. Kp改到0.7， Kd改到0.1 无明显变化，可以尝试更大的倍数。 - 分析：其实目前已经用了最大速度，所以更改Kp， Kd值没有效果，转弯的时候一个轮停住，另一个动，是自然的结果 - 结果：最终，取消90的速度限制，车轮可以在转弯时反转。 - - - + 2. Kp改到0.7， Kd改到0.1 无明显变化，可以尝试更大的倍数。 + + 分析：其实目前已经用了最大速度，所以更改Kp， Kd值没有效果，转弯的时候一个轮停住，另一个动，是自然的结果。 + + 结果：最终，取消90的速度限制，车轮可以在转弯时反转。 + ## 关于dead end的两种方案： 1. 用1个ultra sound传感器，当reflectance sensors检测到全为0或者全1000的时候，检测距离，在范围内就可以判断为dead end； From 89dfd117406c774cdecd371e4b30a483db5d2f47 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 23:30:16 +0100 Subject: [PATCH 31/38] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 3b3fe5e..ef17440 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ * 面包板一侧的负极一条公共地，正极一条全部是电动机的5v供电； * LED校准状态指示灯直接连Arduino的pin13； * MOtor的pin11左 pin12右； -* Reflectance sensor的1-6对应Arduino的pin8-3，保证传感器（小车）的左右和屏幕上的左右是一致的。 +* Reflectance sensor的1-6对应Arduino的pin8-3，**保证传感器（小车）的左右和屏幕上的左右是一致的**。 ## 解决PID和landscape不兼容的问题 From cebd1d281ccea71fd1de5d55a506565ccf343e8f Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 23:33:35 +0100 Subject: [PATCH 32/38] Update README.md --- README.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index ef17440..366f39b 100644 --- a/README.md +++ b/README.md @@ -10,11 +10,13 @@ ## 解决PID和landscape不兼容的问题 -PID算法工作是基于sensor的1-6， 接Arduino的8-3; -Arduino的3-8一定对应屏幕上的sensorValues0-5; + 分析：PID算法工作是基于sensor的1-6， 接Arduino的8-3; + Arduino的3-8一定对应屏幕上的sensorValues0-5; -landscape算法基于sensor的1-6， 接Arduino的3-8； -将两者合并，最简单的办法是更改landscape的判断条件。 + landscape算法基于sensor的1-6， 接Arduino的3-8； + 将两者合并，最简单的办法是更改landscape的判断条件。 + 你问我为什么不改PID？ + 因为如果按照landscape的接线，PID不仅error的正负反了，而且sensorvalues的权重也反了，这样Kp和Kd也得调。 **引脚对应情况** 车头向前，从左到右： @@ -28,7 +30,7 @@ eg. 遇到左转弯，左边黑，右边白， 这样空间的左右和sensorval sensor pin| 6| 5| 4| 3| 2| 1 --|--|--|--|--|--|-- -[i]|0| 1| 2| 3| 4| 5 +i|0| 1| 2| 3| 4| 5 sensorValuse[i]|1| 1| 1| 1| 0| 0| **结论**：所以对landscape修改，T不变，左右转要对调即可。 From ecdebb06d95068daafe70746bec7d905b91fc8b5 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Tue, 11 Dec 2018 23:34:37 +0100 Subject: [PATCH 33/38] Update README.md --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 366f39b..f10d2bb 100644 --- a/README.md +++ b/README.md @@ -37,14 +37,14 @@ i|0| 1| 2| 3| 4| 5 ## 改善转弯表现 - 目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯转弯半径大的原因是传感器的位置不够靠前，解决办法：把传感器向前固定。 - 实验发现： + 目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯转弯半径大的原因是传感器的位置不够靠前，解决办法：把传感器向前固定。 + 实验发现： 1. 速度调小到135效果更差； 2. Kp改到0.7， Kd改到0.1 无明显变化，可以尝试更大的倍数。 - 分析：其实目前已经用了最大速度，所以更改Kp， Kd值没有效果，转弯的时候一个轮停住，另一个动，是自然的结果。 + 分析：其实目前已经用了最大速度，所以更改Kp， Kd值没有效果，转弯的时候一个轮停住，另一个动，是自然的结果。 - 结果：最终，取消90的速度限制，车轮可以在转弯时反转。 + 结果：最终，取消90的速度限制，车轮可以在转弯时反转。 ## 关于dead end的两种方案： From 7db18bc03cce80863fc7e48f050050e40bfc132c Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Thu, 13 Dec 2018 21:58:41 +0100 Subject: [PATCH 34/38] Add files via upload --- sketch_PID_landscape_robust.ino | 319 ++++++++++++++++++++++++++++++++ 1 file changed, 319 insertions(+) create mode 100644 sketch_PID_landscape_robust.ino diff --git a/sketch_PID_landscape_robust.ino b/sketch_PID_landscape_robust.ino new file mode 100644 index 0000000..186f851 --- /dev/null +++ b/sketch_PID_landscape_robust.ino @@ -0,0 +1,319 @@ +#include +#include +#include // Pololu QTR Sensor Library. +//based on Reflectance Sesors's 1~6 connected to Arduino's 8~3 +//有的时候能用，用的时候不能，为什么？不好的的时候，甚至都不拐弯，直接撞墙。 + +Servo left_motor, right_motor; +const int TrigPin = 9; +const int EchoPin = 10; +const int left_motorPin = 11; +const int right_motorPin = 12; +UltraSonicDistanceSensor distanceSensor(TrigPin, EchoPin); +double distance; + +#define KP 0.1 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed +#define KD 0.7 //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd) +#define left_motor_default_speed 180 //normal speed of the left_motor +#define right_motor_default_speed 180 //normal speed of the right_motor +#define left_motor_max_speed 180 //max. speed of the left_motor +#define right_motor_max_speed 180 //max. speed of the right_motor + +#define NUM_SENSORS 6 //number of sensors used +#define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low +#define EMITTER_PIN 8 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 + + +// sensors are connected to digital pins 2 to 7 +QTRSensorsRC qtrrc((unsigned char[]) { 3, 4, 5, 6, 7, 8}, // 3号传感器对应最左边or最右边要测清楚？ + NUM_SENSORS, TIMEOUT, EMITTER_PIN); + +unsigned int sensorValues[NUM_SENSORS]; +int error = 0; +int lastError = 0; +int type = 0; + +void setup() { + Serial.begin(9600); // 波特率 + left_motor.attach(left_motorPin); + right_motor.attach(right_motorPin); + set_motors(90,90); // set the motors not moving + delay(1500); + manual_calibration(); +} + + + +void loop() { + distance = distanceSensor.measureDistanceCm(); + + + //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, + //rightandstraight + int position = qtrrc.readLine(sensorValues); + Serial.print(landscape()); + Serial.print('\n'); + /* + switch(type){ + case 1: + { + test_blink(1); + break; + } + case 2:{ + test_blink(2); + break; + } + case 3: { + test_blink(3); + break; + } + case 4:{ + test_blink(4); + break; + } + case 5:{ + test_blink(5); + break; + } + case 6:{ + test_blink(6); + break; + } + case 7: break; // 不考虑 U turn的话，应该是直线，交给PID + } + */ + // dead end should be checked if there's a wall or line removed + //check_dead_end(); + + int error = -2500 + position; + //换掉左右不仅改变了正负，还改变了每个传感器的权值，要么改Kp， Kd,要么需要把后面左右判断全改了 + int motorSpeed = KP * error + KD * (error - lastError); + lastError = error; + + int leftMotorSpeed = 180 - (left_motor_default_speed + motorSpeed); // closer to 0, speed up + int rightMotorSpeed = right_motor_default_speed - motorSpeed; // closer to 180, speed up, so slow down here + + set_motors(leftMotorSpeed, rightMotorSpeed); +} + +void set_motors(int left_motorspeed, int right_motorspeed) { + // limit motor's speed to 0~90, 90~180 + if (180 - left_motorspeed > left_motor_max_speed ) left_motorspeed = 180 - left_motor_max_speed; // if speed is negative, use 0 instead + if (right_motorspeed > right_motor_max_speed ) right_motorspeed = right_motor_max_speed; + // there's no need to ban spin backward, it allows it turn left or right better. + //if ( left_motorspeed >= 90) left_motorspeed = 90; // avoid spinning backward + //if (right_motorspeed <= 90) right_motorspeed = 90; + left_motor.write(left_motorspeed); + right_motor.write(right_motorspeed); +} + + +void manual_calibration() { + //calibrate for sometime by sliding the sensors across the line, + //or you may use auto-calibration instead + + pinMode(13, OUTPUT); + digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode + //void emittersOn(); + for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds + { + qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call) + } + digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration + // print the calibration minimum values measured when emitters were on + for (int i = 0; i < NUM_SENSORS; i++){ + Serial.print(qtrrc.calibratedMinimumOn[i]); + Serial.print(' '); + } + Serial.println(); + // print the calibration maximum values measured when emitters were on + for (int i = 0; i < NUM_SENSORS; i++){ + Serial.print(qtrrc.calibratedMaximumOn[i]); + Serial.print(' '); + } + Serial.println(); +} + + +int landscape(){ + // first make sure the car is online, then judge the landscape + // use ultra sound sensors to distinguish 'only right' and 'right or straight' + // also for 'T' and '十' + //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, + //rightandstraight + //int position = qtrrc.readLine(sensorValues); + // for debugging + for (unsigned int i = 0;i < NUM_SENSORS; i++){ + Serial.print(i); + Serial.print(' '); + Serial.print(sensorValues[i]); + Serial.print('\t'); + } + + // 实际中需要往前跑一段再判断 + + if(left_judge()){ + Serial.print(666); + Serial.print(' '); + if (wall_judge()) return 1; // use true instead of wall_judge() + else return 5; + } + + if(right_judge()) { + Serial.print(777); + Serial.print(' '); + if (wall_judge()) return 2; + else return 6; + } + + if(T_judge()){ + Serial.print(888); + Serial.print(' '); + if (wall_judge()) return 3; + else return 4; + } + + return 7; // 用于调试 +} + + +bool sensor_judge(unsigned int num, int range, int error){ + int a = sensorValues[num]; + if ((a <= range + error) && (a >= range - error)) return true; + else return false; + } + +bool left_judge(){ + // tell onlyleft from 'left and straight' + // the sensor values should be {1000, 1000, 1000, 1000, 0, 0}， 1号传感器对应的sensorValues[5] + Serial.print(12); + Serial.print(' '); + if ((sensorValues[0]!= 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; + else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] == 0)\ + && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; + else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] != 0) && (sensorValues[5] == 0)) return true; + else return false;/* + bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); + + bool c = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool d = sensor_judge(3, 125, 125) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); + + bool e = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool f = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 125, 125); + //按正常接线顺序，sensor_judge的顺序是0-5 + return (a && b) || (c && d) || (e && f);*/ + } + +bool right_judge(){ + // tell onlylright from 'right and straight' + // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} + Serial.print(34); + Serial.print(' '); + if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; + else if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] == 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; + else if ((sensorValues[0] == 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; + else return false;/* + + bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 350, 650) && sensor_judge(5, 650, 350); + + bool c = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 125, 125); + bool d = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); + + bool e = sensor_judge(0, 125, 125) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool f = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); + return (a && b) || (c && d) || (e && f);*/ + } + +bool T_judge(){ + // tell T from 十 + // the sensor values should be all 1000 + Serial.print(56); + Serial.print(' '); + if ((sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0)\ + && (sensorValues[0] != 0) && (sensorValues[0] != 0)) return true; + else return false;/* + bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); + return a && b; */ + } + + +bool wall_judge(){ + // use ultra_sound to judge if there is a wall in fornt + // return true if there's a wall in the front + + if((distance>10) && (distance<20)){ + return true; + } + + else{ + return false; + } + + } + + +void right_turn(){ + left_motor.write(0); //左轮正转 + right_motor.write(0); //右轮反转 + delay(2150/2); + return; +} + +void left_turn(){ + left_motor.write(180); //左轮正转 + right_motor.write(180); //右轮反转 + delay(2150/2); + return; +} + +void go_straight(){ + left_motor.write(0); //左轮正转 + right_motor.write(180); + delay(1000); + return; + } + +void test_blink(int number){ + for (int i = 0; i< number; i ++){ + digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level) + delay(1000); // wait for a second + digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW + delay(1000); + } + return; + } + +/* +bool check_reflectance_dead_end(int position){ + int lower_bound = 2300; + int higher_bound = 2700; + if (position > 2300 and position < 2700){ + return true; + } + return false; +} + +void check_u_turn(int position){ + u_turn = false; // we assume we shouldn't do u-turn + // stage1: check for reflectance bounds in a fixed range (determined by testing) + u_turn = check_reflectance_dead_end(position); + // stage2: check for ultra-sound sensor (maybe a variable that has been set before) + + return u_turn; +} + +void u_turn(){ + motor1.write(0); //左轮正转 + motor2.write(0); //右轮反转 + delay(2150); + return; +}*/ From 46326f518fb8e6bd52818ea9cc77936990500a02 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Thu, 13 Dec 2018 21:59:20 +0100 Subject: [PATCH 35/38] Delete sketch_PID_landscape_robust.ino --- sketch_PID_landscape_robust.ino | 319 -------------------------------- 1 file changed, 319 deletions(-) delete mode 100644 sketch_PID_landscape_robust.ino diff --git a/sketch_PID_landscape_robust.ino b/sketch_PID_landscape_robust.ino deleted file mode 100644 index 186f851..0000000 --- a/sketch_PID_landscape_robust.ino +++ /dev/null @@ -1,319 +0,0 @@ -#include -#include -#include // Pololu QTR Sensor Library. -//based on Reflectance Sesors's 1~6 connected to Arduino's 8~3 -//有的时候能用，用的时候不能，为什么？不好的的时候，甚至都不拐弯，直接撞墙。 - -Servo left_motor, right_motor; -const int TrigPin = 9; -const int EchoPin = 10; -const int left_motorPin = 11; -const int right_motorPin = 12; -UltraSonicDistanceSensor distanceSensor(TrigPin, EchoPin); -double distance; - -#define KP 0.1 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed -#define KD 0.7 //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd) -#define left_motor_default_speed 180 //normal speed of the left_motor -#define right_motor_default_speed 180 //normal speed of the right_motor -#define left_motor_max_speed 180 //max. speed of the left_motor -#define right_motor_max_speed 180 //max. speed of the right_motor - -#define NUM_SENSORS 6 //number of sensors used -#define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low -#define EMITTER_PIN 8 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 - - -// sensors are connected to digital pins 2 to 7 -QTRSensorsRC qtrrc((unsigned char[]) { 3, 4, 5, 6, 7, 8}, // 3号传感器对应最左边or最右边要测清楚？ - NUM_SENSORS, TIMEOUT, EMITTER_PIN); - -unsigned int sensorValues[NUM_SENSORS]; -int error = 0; -int lastError = 0; -int type = 0; - -void setup() { - Serial.begin(9600); // 波特率 - left_motor.attach(left_motorPin); - right_motor.attach(right_motorPin); - set_motors(90,90); // set the motors not moving - delay(1500); - manual_calibration(); -} - - - -void loop() { - distance = distanceSensor.measureDistanceCm(); - - - //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, - //rightandstraight - int position = qtrrc.readLine(sensorValues); - Serial.print(landscape()); - Serial.print('\n'); - /* - switch(type){ - case 1: - { - test_blink(1); - break; - } - case 2:{ - test_blink(2); - break; - } - case 3: { - test_blink(3); - break; - } - case 4:{ - test_blink(4); - break; - } - case 5:{ - test_blink(5); - break; - } - case 6:{ - test_blink(6); - break; - } - case 7: break; // 不考虑 U turn的话，应该是直线，交给PID - } - */ - // dead end should be checked if there's a wall or line removed - //check_dead_end(); - - int error = -2500 + position; - //换掉左右不仅改变了正负，还改变了每个传感器的权值，要么改Kp， Kd,要么需要把后面左右判断全改了 - int motorSpeed = KP * error + KD * (error - lastError); - lastError = error; - - int leftMotorSpeed = 180 - (left_motor_default_speed + motorSpeed); // closer to 0, speed up - int rightMotorSpeed = right_motor_default_speed - motorSpeed; // closer to 180, speed up, so slow down here - - set_motors(leftMotorSpeed, rightMotorSpeed); -} - -void set_motors(int left_motorspeed, int right_motorspeed) { - // limit motor's speed to 0~90, 90~180 - if (180 - left_motorspeed > left_motor_max_speed ) left_motorspeed = 180 - left_motor_max_speed; // if speed is negative, use 0 instead - if (right_motorspeed > right_motor_max_speed ) right_motorspeed = right_motor_max_speed; - // there's no need to ban spin backward, it allows it turn left or right better. - //if ( left_motorspeed >= 90) left_motorspeed = 90; // avoid spinning backward - //if (right_motorspeed <= 90) right_motorspeed = 90; - left_motor.write(left_motorspeed); - right_motor.write(right_motorspeed); -} - - -void manual_calibration() { - //calibrate for sometime by sliding the sensors across the line, - //or you may use auto-calibration instead - - pinMode(13, OUTPUT); - digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode - //void emittersOn(); - for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds - { - qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call) - } - digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration - // print the calibration minimum values measured when emitters were on - for (int i = 0; i < NUM_SENSORS; i++){ - Serial.print(qtrrc.calibratedMinimumOn[i]); - Serial.print(' '); - } - Serial.println(); - // print the calibration maximum values measured when emitters were on - for (int i = 0; i < NUM_SENSORS; i++){ - Serial.print(qtrrc.calibratedMaximumOn[i]); - Serial.print(' '); - } - Serial.println(); -} - - -int landscape(){ - // first make sure the car is online, then judge the landscape - // use ultra sound sensors to distinguish 'only right' and 'right or straight' - // also for 'T' and '十' - //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, - //rightandstraight - //int position = qtrrc.readLine(sensorValues); - // for debugging - for (unsigned int i = 0;i < NUM_SENSORS; i++){ - Serial.print(i); - Serial.print(' '); - Serial.print(sensorValues[i]); - Serial.print('\t'); - } - - // 实际中需要往前跑一段再判断 - - if(left_judge()){ - Serial.print(666); - Serial.print(' '); - if (wall_judge()) return 1; // use true instead of wall_judge() - else return 5; - } - - if(right_judge()) { - Serial.print(777); - Serial.print(' '); - if (wall_judge()) return 2; - else return 6; - } - - if(T_judge()){ - Serial.print(888); - Serial.print(' '); - if (wall_judge()) return 3; - else return 4; - } - - return 7; // 用于调试 -} - - -bool sensor_judge(unsigned int num, int range, int error){ - int a = sensorValues[num]; - if ((a <= range + error) && (a >= range - error)) return true; - else return false; - } - -bool left_judge(){ - // tell onlyleft from 'left and straight' - // the sensor values should be {1000, 1000, 1000, 1000, 0, 0}， 1号传感器对应的sensorValues[5] - Serial.print(12); - Serial.print(' '); - if ((sensorValues[0]!= 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; - else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] == 0)\ - && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; - else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] != 0) && (sensorValues[5] == 0)) return true; - else return false;/* - bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); - bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); - - bool c = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); - bool d = sensor_judge(3, 125, 125) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); - - bool e = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); - bool f = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 125, 125); - //按正常接线顺序，sensor_judge的顺序是0-5 - return (a && b) || (c && d) || (e && f);*/ - } - -bool right_judge(){ - // tell onlylright from 'right and straight' - // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} - Serial.print(34); - Serial.print(' '); - if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; - else if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] == 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; - else if ((sensorValues[0] == 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; - else return false;/* - - bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 650, 350); - bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 350, 650) && sensor_judge(5, 650, 350); - - bool c = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 125, 125); - bool d = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); - - bool e = sensor_judge(0, 125, 125) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); - bool f = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); - return (a && b) || (c && d) || (e && f);*/ - } - -bool T_judge(){ - // tell T from 十 - // the sensor values should be all 1000 - Serial.print(56); - Serial.print(' '); - if ((sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0)\ - && (sensorValues[0] != 0) && (sensorValues[0] != 0)) return true; - else return false;/* - bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); - bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); - return a && b; */ - } - - -bool wall_judge(){ - // use ultra_sound to judge if there is a wall in fornt - // return true if there's a wall in the front - - if((distance>10) && (distance<20)){ - return true; - } - - else{ - return false; - } - - } - - -void right_turn(){ - left_motor.write(0); //左轮正转 - right_motor.write(0); //右轮反转 - delay(2150/2); - return; -} - -void left_turn(){ - left_motor.write(180); //左轮正转 - right_motor.write(180); //右轮反转 - delay(2150/2); - return; -} - -void go_straight(){ - left_motor.write(0); //左轮正转 - right_motor.write(180); - delay(1000); - return; - } - -void test_blink(int number){ - for (int i = 0; i< number; i ++){ - digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level) - delay(1000); // wait for a second - digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW - delay(1000); - } - return; - } - -/* -bool check_reflectance_dead_end(int position){ - int lower_bound = 2300; - int higher_bound = 2700; - if (position > 2300 and position < 2700){ - return true; - } - return false; -} - -void check_u_turn(int position){ - u_turn = false; // we assume we shouldn't do u-turn - // stage1: check for reflectance bounds in a fixed range (determined by testing) - u_turn = check_reflectance_dead_end(position); - // stage2: check for ultra-sound sensor (maybe a variable that has been set before) - - return u_turn; -} - -void u_turn(){ - motor1.write(0); //左轮正转 - motor2.write(0); //右轮反转 - delay(2150); - return; -}*/ From ddcb721a9acff80b3f79bbd1f2f4f309ed0be0d9 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Thu, 13 Dec 2018 21:59:33 +0100 Subject: [PATCH 36/38] Add files via upload --- sketch_PID_landscape_robust.ino | 319 ++++++++++++++++++++++++++++++++ 1 file changed, 319 insertions(+) create mode 100644 sketch_PID_landscape_robust.ino diff --git a/sketch_PID_landscape_robust.ino b/sketch_PID_landscape_robust.ino new file mode 100644 index 0000000..186f851 --- /dev/null +++ b/sketch_PID_landscape_robust.ino @@ -0,0 +1,319 @@ +#include +#include +#include // Pololu QTR Sensor Library. +//based on Reflectance Sesors's 1~6 connected to Arduino's 8~3 +//有的时候能用，用的时候不能，为什么？不好的的时候，甚至都不拐弯，直接撞墙。 + +Servo left_motor, right_motor; +const int TrigPin = 9; +const int EchoPin = 10; +const int left_motorPin = 11; +const int right_motorPin = 12; +UltraSonicDistanceSensor distanceSensor(TrigPin, EchoPin); +double distance; + +#define KP 0.1 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed +#define KD 0.7 //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd) +#define left_motor_default_speed 180 //normal speed of the left_motor +#define right_motor_default_speed 180 //normal speed of the right_motor +#define left_motor_max_speed 180 //max. speed of the left_motor +#define right_motor_max_speed 180 //max. speed of the right_motor + +#define NUM_SENSORS 6 //number of sensors used +#define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low +#define EMITTER_PIN 8 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 + + +// sensors are connected to digital pins 2 to 7 +QTRSensorsRC qtrrc((unsigned char[]) { 3, 4, 5, 6, 7, 8}, // 3号传感器对应最左边or最右边要测清楚？ + NUM_SENSORS, TIMEOUT, EMITTER_PIN); + +unsigned int sensorValues[NUM_SENSORS]; +int error = 0; +int lastError = 0; +int type = 0; + +void setup() { + Serial.begin(9600); // 波特率 + left_motor.attach(left_motorPin); + right_motor.attach(right_motorPin); + set_motors(90,90); // set the motors not moving + delay(1500); + manual_calibration(); +} + + + +void loop() { + distance = distanceSensor.measureDistanceCm(); + + + //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, + //rightandstraight + int position = qtrrc.readLine(sensorValues); + Serial.print(landscape()); + Serial.print('\n'); + /* + switch(type){ + case 1: + { + test_blink(1); + break; + } + case 2:{ + test_blink(2); + break; + } + case 3: { + test_blink(3); + break; + } + case 4:{ + test_blink(4); + break; + } + case 5:{ + test_blink(5); + break; + } + case 6:{ + test_blink(6); + break; + } + case 7: break; // 不考虑 U turn的话，应该是直线，交给PID + } + */ + // dead end should be checked if there's a wall or line removed + //check_dead_end(); + + int error = -2500 + position; + //换掉左右不仅改变了正负，还改变了每个传感器的权值，要么改Kp， Kd,要么需要把后面左右判断全改了 + int motorSpeed = KP * error + KD * (error - lastError); + lastError = error; + + int leftMotorSpeed = 180 - (left_motor_default_speed + motorSpeed); // closer to 0, speed up + int rightMotorSpeed = right_motor_default_speed - motorSpeed; // closer to 180, speed up, so slow down here + + set_motors(leftMotorSpeed, rightMotorSpeed); +} + +void set_motors(int left_motorspeed, int right_motorspeed) { + // limit motor's speed to 0~90, 90~180 + if (180 - left_motorspeed > left_motor_max_speed ) left_motorspeed = 180 - left_motor_max_speed; // if speed is negative, use 0 instead + if (right_motorspeed > right_motor_max_speed ) right_motorspeed = right_motor_max_speed; + // there's no need to ban spin backward, it allows it turn left or right better. + //if ( left_motorspeed >= 90) left_motorspeed = 90; // avoid spinning backward + //if (right_motorspeed <= 90) right_motorspeed = 90; + left_motor.write(left_motorspeed); + right_motor.write(right_motorspeed); +} + + +void manual_calibration() { + //calibrate for sometime by sliding the sensors across the line, + //or you may use auto-calibration instead + + pinMode(13, OUTPUT); + digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode + //void emittersOn(); + for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds + { + qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call) + } + digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration + // print the calibration minimum values measured when emitters were on + for (int i = 0; i < NUM_SENSORS; i++){ + Serial.print(qtrrc.calibratedMinimumOn[i]); + Serial.print(' '); + } + Serial.println(); + // print the calibration maximum values measured when emitters were on + for (int i = 0; i < NUM_SENSORS; i++){ + Serial.print(qtrrc.calibratedMaximumOn[i]); + Serial.print(' '); + } + Serial.println(); +} + + +int landscape(){ + // first make sure the car is online, then judge the landscape + // use ultra sound sensors to distinguish 'only right' and 'right or straight' + // also for 'T' and '十' + //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, + //rightandstraight + //int position = qtrrc.readLine(sensorValues); + // for debugging + for (unsigned int i = 0;i < NUM_SENSORS; i++){ + Serial.print(i); + Serial.print(' '); + Serial.print(sensorValues[i]); + Serial.print('\t'); + } + + // 实际中需要往前跑一段再判断 + + if(left_judge()){ + Serial.print(666); + Serial.print(' '); + if (wall_judge()) return 1; // use true instead of wall_judge() + else return 5; + } + + if(right_judge()) { + Serial.print(777); + Serial.print(' '); + if (wall_judge()) return 2; + else return 6; + } + + if(T_judge()){ + Serial.print(888); + Serial.print(' '); + if (wall_judge()) return 3; + else return 4; + } + + return 7; // 用于调试 +} + + +bool sensor_judge(unsigned int num, int range, int error){ + int a = sensorValues[num]; + if ((a <= range + error) && (a >= range - error)) return true; + else return false; + } + +bool left_judge(){ + // tell onlyleft from 'left and straight' + // the sensor values should be {1000, 1000, 1000, 1000, 0, 0}， 1号传感器对应的sensorValues[5] + Serial.print(12); + Serial.print(' '); + if ((sensorValues[0]!= 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; + else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] == 0)\ + && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; + else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] != 0) && (sensorValues[5] == 0)) return true; + else return false;/* + bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); + + bool c = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool d = sensor_judge(3, 125, 125) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); + + bool e = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool f = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 125, 125); + //按正常接线顺序，sensor_judge的顺序是0-5 + return (a && b) || (c && d) || (e && f);*/ + } + +bool right_judge(){ + // tell onlylright from 'right and straight' + // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} + Serial.print(34); + Serial.print(' '); + if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; + else if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] == 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; + else if ((sensorValues[0] == 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ + && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; + else return false;/* + + bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 350, 650) && sensor_judge(5, 650, 350); + + bool c = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 125, 125); + bool d = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); + + bool e = sensor_judge(0, 125, 125) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool f = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); + return (a && b) || (c && d) || (e && f);*/ + } + +bool T_judge(){ + // tell T from 十 + // the sensor values should be all 1000 + Serial.print(56); + Serial.print(' '); + if ((sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0)\ + && (sensorValues[0] != 0) && (sensorValues[0] != 0)) return true; + else return false;/* + bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); + return a && b; */ + } + + +bool wall_judge(){ + // use ultra_sound to judge if there is a wall in fornt + // return true if there's a wall in the front + + if((distance>10) && (distance<20)){ + return true; + } + + else{ + return false; + } + + } + + +void right_turn(){ + left_motor.write(0); //左轮正转 + right_motor.write(0); //右轮反转 + delay(2150/2); + return; +} + +void left_turn(){ + left_motor.write(180); //左轮正转 + right_motor.write(180); //右轮反转 + delay(2150/2); + return; +} + +void go_straight(){ + left_motor.write(0); //左轮正转 + right_motor.write(180); + delay(1000); + return; + } + +void test_blink(int number){ + for (int i = 0; i< number; i ++){ + digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level) + delay(1000); // wait for a second + digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW + delay(1000); + } + return; + } + +/* +bool check_reflectance_dead_end(int position){ + int lower_bound = 2300; + int higher_bound = 2700; + if (position > 2300 and position < 2700){ + return true; + } + return false; +} + +void check_u_turn(int position){ + u_turn = false; // we assume we shouldn't do u-turn + // stage1: check for reflectance bounds in a fixed range (determined by testing) + u_turn = check_reflectance_dead_end(position); + // stage2: check for ultra-sound sensor (maybe a variable that has been set before) + + return u_turn; +} + +void u_turn(){ + motor1.write(0); //左轮正转 + motor2.write(0); //右轮反转 + delay(2150); + return; +}*/ From 9d73ae020ffabb547ad7668706f083ab1c782c84 Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Thu, 13 Dec 2018 22:00:26 +0100 Subject: [PATCH 37/38] Update sketch_PID_landscape_robust.ino --- sketch_PID_landscape_robust.ino | 58 +++++++++++++++++++-------------- 1 file changed, 33 insertions(+), 25 deletions(-) diff --git a/sketch_PID_landscape_robust.ino b/sketch_PID_landscape_robust.ino index 186f851..8740d5b 100644 --- a/sketch_PID_landscape_robust.ino +++ b/sketch_PID_landscape_robust.ino @@ -22,7 +22,8 @@ double distance; #define NUM_SENSORS 6 //number of sensors used #define TIMEOUT 2500 //waits for 2500 us for sensor outputs to go low #define EMITTER_PIN 8 //emitterPin is the Arduino digital pin that controls whether the IR LEDs are on or off. Emitter is controlled by digital pin 2 - +#define RangeB 625 +#define ErrorB 375 // sensors are connected to digital pins 2 to 7 QTRSensorsRC qtrrc((unsigned char[]) { 3, 4, 5, 6, 7, 8}, // 3号传感器对应最左边or最右边要测清楚？ @@ -32,8 +33,10 @@ unsigned int sensorValues[NUM_SENSORS]; int error = 0; int lastError = 0; int type = 0; +int i=0; void setup() { + Serial.begin(9600); // 波特率 left_motor.attach(left_motorPin); right_motor.attach(right_motorPin); @@ -45,14 +48,16 @@ void setup() { void loop() { + i++; distance = distanceSensor.measureDistanceCm(); //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, //rightandstraight int position = qtrrc.readLine(sensorValues); - Serial.print(landscape()); - Serial.print('\n'); + + if(i>200){Serial.print(landscape()); + Serial.print('\n');} /* switch(type){ case 1: @@ -154,6 +159,7 @@ int landscape(){ // 实际中需要往前跑一段再判断 if(left_judge()){ + i=0; Serial.print(666); Serial.print(' '); if (wall_judge()) return 1; // use true instead of wall_judge() @@ -161,6 +167,7 @@ int landscape(){ } if(right_judge()) { + i=0; Serial.print(777); Serial.print(' '); if (wall_judge()) return 2; @@ -168,6 +175,7 @@ int landscape(){ } if(T_judge()){ + i=0; Serial.print(888); Serial.print(' '); if (wall_judge()) return 3; @@ -189,60 +197,60 @@ bool left_judge(){ // the sensor values should be {1000, 1000, 1000, 1000, 0, 0}， 1号传感器对应的sensorValues[5] Serial.print(12); Serial.print(' '); - if ((sensorValues[0]!= 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ + /* if ((sensorValues[0]!= 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] == 0)\ && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ && (sensorValues[4] != 0) && (sensorValues[5] == 0)) return true; - else return false;/* - bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); - bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); + else return false;*/ + bool a = sensor_judge(0, RangeB, ErrorB) && sensor_judge(1, RangeB, ErrorB) && sensor_judge(2, RangeB, ErrorB); + bool b = sensor_judge(3, RangeB, ErrorB) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); - bool c = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); + bool c = sensor_judge(0, RangeB, ErrorB) && sensor_judge(1, RangeB, ErrorB) && sensor_judge(2, RangeB, ErrorB); bool d = sensor_judge(3, 125, 125) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); - bool e = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); - bool f = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 125, 125); + bool e = sensor_judge(0, RangeB, ErrorB) && sensor_judge(1,RangeB, ErrorB) && sensor_judge(2, RangeB, ErrorB); + bool f = sensor_judge(3, RangeB, ErrorB) && sensor_judge(4, RangeB, ErrorB) && sensor_judge(5, 125, 125); //按正常接线顺序，sensor_judge的顺序是0-5 - return (a && b) || (c && d) || (e && f);*/ + return (a && b) || (c && d) || (e && f); } bool right_judge(){ // tell onlylright from 'right and straight' // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} Serial.print(34); - Serial.print(' '); + Serial.print(' ');/* if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; else if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] == 0) && (sensorValues[3] != 0)\ && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; else if ((sensorValues[0] == 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; - else return false;/* + else return false;*/ - bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 650, 350); - bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 350, 650) && sensor_judge(5, 650, 350); + bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, RangeB, ErrorB); + bool b = sensor_judge(3, RangeB, ErrorB) && sensor_judge(4, RangeB, ErrorB) && sensor_judge(5,RangeB, ErrorB); bool c = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 125, 125); - bool d = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); + bool d = sensor_judge(3, RangeB, ErrorB) && sensor_judge(4, RangeB, ErrorB) && sensor_judge(5, RangeB, ErrorB); - bool e = sensor_judge(0, 125, 125) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); - bool f = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); - return (a && b) || (c && d) || (e && f);*/ + bool e = sensor_judge(0, 125, 125) && sensor_judge(1, RangeB, ErrorB) && sensor_judge(2, RangeB, ErrorB); + bool f = sensor_judge(3, RangeB, ErrorB) && sensor_judge(4, RangeB, ErrorB) && sensor_judge(5, RangeB, ErrorB); + return (a && b) || (c && d) || (e && f); } bool T_judge(){ // tell T from 十 // the sensor values should be all 1000 Serial.print(56); - Serial.print(' '); + Serial.print(' ');/* if ((sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0)\ && (sensorValues[0] != 0) && (sensorValues[0] != 0)) return true; - else return false;/* - bool a = sensor_judge(0, 650, 350) && sensor_judge(1, 650, 350) && sensor_judge(2, 650, 350); - bool b = sensor_judge(3, 650, 350) && sensor_judge(4, 650, 350) && sensor_judge(5, 650, 350); - return a && b; */ + else return false;*/ + bool a = sensor_judge(0, RangeB, ErrorB) && sensor_judge(1, RangeB, ErrorB) && sensor_judge(2, RangeB, ErrorB); + bool b = sensor_judge(3, RangeB, ErrorB) && sensor_judge(4, RangeB, ErrorB) && sensor_judge(5, RangeB, ErrorB); + return a && b; } @@ -250,7 +258,7 @@ bool wall_judge(){ // use ultra_sound to judge if there is a wall in fornt // return true if there's a wall in the front - if((distance>10) && (distance<20)){ + if((distance>10) && (distance<25)){ return true; } From de726e068a49555a721a314c02fd304c70e6821c Mon Sep 17 00:00:00 2001 From: Leo Xiong <39155356+XZLeo@users.noreply.github.com> Date: Fri, 14 Dec 2018 18:15:06 +0100 Subject: [PATCH 38/38] Update sketch_PID_landscape_robust.ino All 9 types of landscapes can be recognized by return integers from 1~9 while running controlled by PID. --- sketch_PID_landscape_robust.ino | 124 +++++++++----------------------- 1 file changed, 33 insertions(+), 91 deletions(-) diff --git a/sketch_PID_landscape_robust.ino b/sketch_PID_landscape_robust.ino index 8740d5b..9422352 100644 --- a/sketch_PID_landscape_robust.ino +++ b/sketch_PID_landscape_robust.ino @@ -12,8 +12,8 @@ const int right_motorPin = 12; UltraSonicDistanceSensor distanceSensor(TrigPin, EchoPin); double distance; -#define KP 0.1 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed -#define KD 0.7 //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd) +#define KP 0.2 //experiment to determine this, start by something sm all that just makes your bot follow the line at a slow speed +#define KD 0.5 //experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd) #define left_motor_default_speed 180 //normal speed of the left_motor #define right_motor_default_speed 180 //normal speed of the right_motor #define left_motor_max_speed 180 //max. speed of the left_motor @@ -35,8 +35,7 @@ int lastError = 0; int type = 0; int i=0; -void setup() { - +void setup() { Serial.begin(9600); // 波特率 left_motor.attach(left_motorPin); right_motor.attach(right_motorPin); @@ -50,44 +49,14 @@ void setup() { void loop() { i++; distance = distanceSensor.measureDistanceCm(); - - //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, //rightandstraight - int position = qtrrc.readLine(sensorValues); - - if(i>200){Serial.print(landscape()); - Serial.print('\n');} - /* - switch(type){ - case 1: - { - test_blink(1); - break; - } - case 2:{ - test_blink(2); - break; - } - case 3: { - test_blink(3); - break; - } - case 4:{ - test_blink(4); - break; - } - case 5:{ - test_blink(5); - break; - } - case 6:{ - test_blink(6); - break; - } - case 7: break; // 不考虑 U turn的话，应该是直线，交给PID - } - */ + int position = qtrrc.readLine(sensorValues); + if(i>200){ + Serial.print(landscape()); + Serial.print('\n'); + } + // dead end should be checked if there's a wall or line removed //check_dead_end(); @@ -117,7 +86,6 @@ void set_motors(int left_motorspeed, int right_motorspeed) { void manual_calibration() { //calibrate for sometime by sliding the sensors across the line, //or you may use auto-calibration instead - pinMode(13, OUTPUT); digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode //void emittersOn(); @@ -142,47 +110,41 @@ void manual_calibration() { int landscape(){ - // first make sure the car is online, then judge the landscape // use ultra sound sensors to distinguish 'only right' and 'right or straight' // also for 'T' and '十' - //1, 2, 3, 4, 5, 6 corresonds to leftonly, rightonly, T, 十, leftandstraight, - //rightandstraight - //int position = qtrrc.readLine(sensorValues); - // for debugging - for (unsigned int i = 0;i < NUM_SENSORS; i++){ + //1, 2, 3, 4, 5, 6, 7, 8, 9 corresonds to leftonly, rightonly, T, 十, leftandstraight, + //rightandstraight, straight, deadend, removedline, straight + for (unsigned int i = 0;i < NUM_SENSORS; i++){ Serial.print(i); Serial.print(' '); Serial.print(sensorValues[i]); Serial.print('\t'); } - - // 实际中需要往前跑一段再判断 - if(left_judge()){ i=0; - Serial.print(666); - Serial.print(' '); if (wall_judge()) return 1; // use true instead of wall_judge() else return 5; } if(right_judge()) { i=0; - Serial.print(777); - Serial.print(' '); if (wall_judge()) return 2; else return 6; } if(T_judge()){ i=0; - Serial.print(888); - Serial.print(' '); if (wall_judge()) return 3; else return 4; + } + + if(dead_end_judge()) { + i = 0; + if (wall_judge()) return 7; + else return 8; } - - return 7; // 用于调试 + + return 9; } @@ -195,15 +157,6 @@ bool sensor_judge(unsigned int num, int range, int error){ bool left_judge(){ // tell onlyleft from 'left and straight' // the sensor values should be {1000, 1000, 1000, 1000, 0, 0}， 1号传感器对应的sensorValues[5] - Serial.print(12); - Serial.print(' '); - /* if ((sensorValues[0]!= 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; - else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] == 0)\ - && (sensorValues[4] == 0) && (sensorValues[5] == 0)) return true; - else if ((sensorValues[0] != 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] != 0) && (sensorValues[5] == 0)) return true; - else return false;*/ bool a = sensor_judge(0, RangeB, ErrorB) && sensor_judge(1, RangeB, ErrorB) && sensor_judge(2, RangeB, ErrorB); bool b = sensor_judge(3, RangeB, ErrorB) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); @@ -218,17 +171,7 @@ bool left_judge(){ bool right_judge(){ // tell onlylright from 'right and straight' - // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} - Serial.print(34); - Serial.print(' ');/* - if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; - else if ((sensorValues[0] == 0) && (sensorValues[1] == 0) && (sensorValues[2] == 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; - else if ((sensorValues[0] == 0) && (sensorValues[1] != 0) && (sensorValues[2] != 0) && (sensorValues[3] != 0)\ - && (sensorValues[4] != 0) && (sensorValues[5] != 0)) return true; - else return false;*/ - + // the sensor values should be{0, 0, 1000, 1000, 1000, 1000} bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, RangeB, ErrorB); bool b = sensor_judge(3, RangeB, ErrorB) && sensor_judge(4, RangeB, ErrorB) && sensor_judge(5,RangeB, ErrorB); @@ -243,11 +186,6 @@ bool right_judge(){ bool T_judge(){ // tell T from 十 // the sensor values should be all 1000 - Serial.print(56); - Serial.print(' ');/* - if ((sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0) && (sensorValues[0] != 0)\ - && (sensorValues[0] != 0) && (sensorValues[0] != 0)) return true; - else return false;*/ bool a = sensor_judge(0, RangeB, ErrorB) && sensor_judge(1, RangeB, ErrorB) && sensor_judge(2, RangeB, ErrorB); bool b = sensor_judge(3, RangeB, ErrorB) && sensor_judge(4, RangeB, ErrorB) && sensor_judge(5, RangeB, ErrorB); return a && b; @@ -257,18 +195,22 @@ bool T_judge(){ bool wall_judge(){ // use ultra_sound to judge if there is a wall in fornt // return true if there's a wall in the front - - if((distance>10) && (distance<25)){ + if((distance>10) && (distance<25)){ return true; - } - + } else{ return false; - } - + } } - +bool dead_end_judge(){ + // 全在0附近的区间里 + bool a = sensor_judge(0, 125, 125) && sensor_judge(1, 125, 125) && sensor_judge(2, 125, 125); + bool b = sensor_judge(3, 125, 125) && sensor_judge(4, 125, 125) && sensor_judge(5, 125, 125); + return a && b; +} + // 把车提起来会返回4，因为全为1000， 且没有墙 +/* void right_turn(){ left_motor.write(0); //左轮正转 right_motor.write(0); //右轮反转 @@ -300,7 +242,7 @@ void test_blink(int number){ return; } -/* + bool check_reflectance_dead_end(int position){ int lower_bound = 2300; int higher_bound = 2700;