Patch 2

PID_landscape

@@ -0,0 +1,242 @@
+#include <Servo.h> 
+#include <HCSR04.h>
+#include <QTRSensors.h> // 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;
+}*/

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。
+   * 优点：比方案一更稳妥；
+   * 缺点：需要两次检测、中间要停下来，更耗时。