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; +}*/ diff --git a/README.md b/README.md index 4f840f9..f10d2bb 100644 --- a/README.md +++ b/README.md @@ -1 +1,57 @@ # arduino-maze-robot +## 记录连线 +**面包板上连线记录！！连好之后勿动，节约每次测试前的时间！！！** +* 传感器的供电全部使用Arduino 5v； +* Motor， graspper lifter的供电直接使用充电宝； +* 面包板一侧的负极一条公共地，正极一条全部是电动机的5v供电； +* LED校准状态指示灯直接连Arduino的pin13； +* MOtor的pin11左 pin12右； +* Reflectance sensor的1-6对应Arduino的pin8-3，**保证传感器（小车）的左右和屏幕上的左右是一致的**。 + + +## 解决PID和landscape不兼容的问题 + 分析：PID算法工作是基于sensor的1-6， 接Arduino的8-3; + Arduino的3-8一定对应屏幕上的sensorValues0-5; + + landscape算法基于sensor的1-6， 接Arduino的3-8； + 将两者合并，最简单的办法是更改landscape的判断条件。 + 你问我为什么不改PID？ + 因为如果按照landscape的接线，PID不仅error的正负反了，而且sensorvalues的权重也反了，这样Kp和Kd也得调。 + +**引脚对应情况** +车头向前，从左到右： + +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 +--|--|--|--|--|--|-- +i|0| 1| 2| 3| 4| 5 + sensorValuse[i]|1| 1| 1| 1| 0| 0| + +**结论**：所以对landscape修改，T不变，左右转要对调即可。 + + +## 改善转弯表现 + 目前转弯的情况基本是：当车头的传感器到达弯道黑胶带处，一个轮子定住，另一个运动打弯转弯半径大的原因是传感器的位置不够靠前，解决办法：把传感器向前固定。 + 实验发现： + 1. 速度调小到135效果更差； + 2. Kp改到0.7， Kd改到0.1 无明显变化，可以尝试更大的倍数。 + + 分析：其实目前已经用了最大速度，所以更改Kp， Kd值没有效果，转弯的时候一个轮停住，另一个动，是自然的结果。 + + 结果：最终，取消90的速度限制，车轮可以在转弯时反转。 + + +## 关于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。 + * 优点：比方案一更稳妥； + * 缺点：需要两次检测、中间要停下来，更耗时。 diff --git a/arduino-maze-robot.ino b/arduino-maze-robot.ino index d614219..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. @@ -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 DEBUG 1 +#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 1 to 6 -QTRSensorsRC qtrrc((unsigned char[]) {1, 2, 3, 4, 5, 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); unsigned int sensorValues[NUM_SENSORS]; @@ -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); @@ -170,3 +173,48 @@ void print_left_right_motorSpeed(int leftMotorSpeed, int rightMotorSpeed){ 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; +} diff --git a/sketch_PID_landscape_robust.ino b/sketch_PID_landscape_robust.ino new file mode 100644 index 0000000..9422352 --- /dev/null +++ b/sketch_PID_landscape_robust.ino @@ -0,0 +1,269 @@ +#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.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 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最右边要测清楚？ + NUM_SENSORS, TIMEOUT, EMITTER_PIN); + +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); + set_motors(90,90); // set the motors not moving + delay(1500); + manual_calibration(); +} + + + +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'); + } + + // 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(){ + // use ultra sound sensors to distinguish 'only right' and 'right or straight' + // also for 'T' and '十' + //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; + if (wall_judge()) return 1; // use true instead of wall_judge() + else return 5; + } + + if(right_judge()) { + i=0; + if (wall_judge()) return 2; + else return 6; + } + + if(T_judge()){ + i=0; + if (wall_judge()) return 3; + else return 4; + } + + if(dead_end_judge()) { + i = 0; + if (wall_judge()) return 7; + else return 8; + } + + return 9; +} + + +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] + 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, 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, 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); + } + +bool right_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, 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, RangeB, ErrorB) && sensor_judge(4, RangeB, ErrorB) && sensor_judge(5, RangeB, ErrorB); + + 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 + 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; + } + + +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)){ + 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); //右轮反转 + 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; +}*/ diff --git a/sketch_landscape.ino b/sketch_landscape.ino new file mode 100644 index 0000000..4c80c49 --- /dev/null +++ b/sketch_landscape.ino @@ -0,0 +1,180 @@ +/* + 这版基于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 +#include // Pololu QTR Sensor Library. + +//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); + +#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[]) { 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); + manual_calibration(); + +} + +void loop() { + double distance = distanceSensor.measureDistanceCm(); + delay(3000); + landscape(); +} + +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); + + Serial.print(left_judge()); + Serial.print('\n'); + /* + if(online_judge()){ + // 实际中需要往前跑一段再判断 + if(left_judge()){ + if (wall_judge()) return 1; // use true instead of wall_judge() + else return 5; + } + + if(wall_judge()) { + if (true) return 2; + else return 6; + } + + if(wall_judge()){ + if (true) return 3; + else return 4; + } + }*/ + } + + +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 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; + } + + +boolean left_judge(){ + // tell onlyleft from 'left and straight' + // the sensor values should be {1000, 1000, 1000, 1000, 0, 0} + 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} + 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 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(double distance){ + // 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) + return 1; + else + return 0; + delay(200); + } 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); +}