rollback

freshman_cup/consts.h

@@ -9,7 +9,6 @@ const unsigned lidartimeout=100; // in ms
 const int servo_offset = 95;  // 用于调整舵机中点
 const float car_left=-100;
 const float car_right=100;
-const float car_front=60;
 const float carlen=50;
 
 // pins

freshman_cup/freshman_cup.ino

@@ -10,8 +10,8 @@ RPLidar lidar;
 Servo servo;
 MotorDriver motor;
 
-extern float dist[360];
-extern bool valid[360];
+float dist[360];
+bool valid[360]={0};
 volatile bool tick=false;
 
 bool IRAM_ATTR TimerHandler(void * timerNo){
@@ -37,16 +37,15 @@ void setup() {
     // 定时中断
     ESP32Timer ITimer(1);
     ITimer.attachInterruptInterval(
-        400000,
+        150000,
         TimerHandler
     );
-    memset(valid, 0, 360*sizeof(bool));
 }
 
 void loop() {
     if(tick) { // call next
         tick=false;
-        auto next_status=next();
+        auto next_status=next(dist, valid);
         #ifdef Debug
         Serial.printf("operation on this loop: (%3.1f, %3.1f)\r\n",
             next_status.angle, next_status.velocity);

freshman_cup/gogogo.cpp

@@ -6,68 +6,30 @@
 // - should run in the middle of left and right bound
 // - have one bound in front, should turn
 
-float dist[360];
-bool valid[360];
-
-const float bound_judge=50;
-
-// angle turning right, 0 if all left of car_left
-float angle_right(int from, int to){
-    float max_r=0;
-    for(int i=from; i<=to; i++){
-        if (!valid[i]) continue;
-        float theta=float(i)*M_PI/180.f;
-        float x=dist[i]*cosf(theta)+car_left;
-        float y=dist[i]*sinf(theta)-car_front;
-        float r=(x*x+y*y)/(2*x);
-        if(r>max_r) max_r=r;
-    }
-    return max_r!=0 ? asinf(carlen/(max_r))/M_PI*180.f : 0;
+/// return x coordinate of polar cord (r, theta)
+inline float x_of(float r, float theta) {
+    return r*cosf(theta*M_PI/180.f);
 }
 
-// angle turning left, 0 if all right of car_right
-float angle_left(int from, int to){
-    float max_r=0;
-    for(int i=from; i<=to; i++){
-        if (!valid[i]) continue;
-        float theta=float(i)*M_PI/180.f;
-        float x=dist[i]*cosf(theta)-car_right;
-        float y=dist[i]*sinf(theta)-car_front;
-        float r=(x*x+y*y)/(-2*x);
-        if(r>max_r) max_r=r;
-    }
-    return max_r!=0 ? asinf(carlen/(max_r))/M_PI*180.f : 0;
-}
+const float bound_judge=20;
 
-// average distance
-float avg_dist(int from, int to){
-    float sum=0;
-    int count=0;
-    for(int i=from; i<=to; i++){
-        if (!valid[i]) continue;
-        count++;
-        sum+=dist[i];
-    }
-    return sum/count;
-}
-
-Go next(){
-    /*
-    for(int i=30; i<=150; i++)
-        if(valid[i]) Serial.printf("%d:%3.1f ", i, dist[i]);
-    Serial.println();
-    */
+Go next(const float* dist, const bool *valid){
     // left boundary
     int bound_left=150;
     while((bound_left)>=30){
-        while (!valid[bound_left]) {bound_left--; continue;}
+        while ((bound_left)>=30 && !valid[bound_left]) bound_left--;
         int leftside=bound_left;
+        bool flag=false;
         float diff=0;
-        while((--bound_left)>=30 && !valid[bound_left]);
-        if(bound_left<30) break;
-        int angle=leftside-bound_left;
-        diff=(dist[leftside]-dist[bound_left])/angle;
-        if(diff>bound_judge || diff<-bound_judge) break; // boundary found
+        while((--bound_left)>=30){
+            if(valid[bound_left]){
+                int angle=leftside-bound_left;
+                diff=(dist[leftside]-dist[bound_left])/angle;
+                flag=true;
+                break;
+            }
+        }
+        if(flag && (diff>bound_judge || diff<-bound_judge)) break; // boundary found
     }
     // boundary not found, turn!
     if (bound_left<30){
@@ -89,42 +51,82 @@ Go next(){
         }
         if(count_left!=0) sum_left/=count_left; // greater is nearer
         if(count_right!=0)sum_right/=count_right;
-        float angle;
-        if(sum_right>sum_left){ // turn left
-            angle=angle_left(30, 150);
-        } else if (sum_right<sum_left){ // turn right
-            angle=-angle_right(30, 150);
-        } else angle=0; // ?
+        float angle=constrain(15000.*(sum_right-sum_left), -10, 10); // TODO
         #ifdef Debug
         Serial.printf("no boundary, turn! sum:(%f, %f), angle: %f\r\n", sum_left, sum_right, angle);
         #endif
-        return Go{speed*0.5f, constrain(angle, -10.f, 10.f)+90}; // ?
+        // correction on speed and angle, based on distance
+        // from 0 to -angle.
+        int to=int(-angle*0.66)+90, count=0;
+        float sum=0;
+        if(angle<0) for(int i=90; i<to; i++) 
+            if(valid[i]) {sum+=dist[i]; count++;}
+        else for(int i=90; i>to; i--)
+            if(valid[i]) {sum+=dist[i]; count++;}
+        /// 100mm as danger zone
+        if(count>0){
+            sum/=count;
+            return Go{speed*constrain(sum*.01f, 0.5f, 1.f), angle*10.f/sqrtf(sum)+90};
+        } else return Go{speed*0.5f, angle+90}; // ?
     }
-    // boundary found, search right bound (which MUST exist)
+    #ifdef Debug
+    Serial.println("boundary found!");
+    #endif
+    // boundary found, find right bound (which MUST exist)
     int bound_right=30;
     while(bound_right<bound_left){
-        while (!valid[bound_right]) {bound_right++; continue;}
+        while ((bound_right<bound_left) && !valid[bound_right]) bound_right++;
         int rightside=bound_right;
-        while((++bound_right)<=bound_left && !valid[bound_right]);
-        if(bound_right>=bound_left) break;
-        int angle=bound_right-rightside;
-        float diff=(dist[rightside]-dist[bound_right])/angle;
-        if((diff>bound_judge || diff<-bound_judge)) {
-            break; // boundary found
+        bool flag=false;
+        float diff=0;
+        while((++bound_right)<=bound_left){
+            if(valid[bound_right]){
+                int angle=bound_right-rightside;
+                diff=(dist[rightside]-dist[bound_left])/angle;
+                flag=true;
+                break;
+            }
         }
+        if(flag && (diff>bound_judge || diff<-bound_judge)) break; // boundary found
     }
     // calculate left and right average distance
-    float aleft=angle_left(30, bound_right);
-    float aright=angle_right(bound_left, 150);
+    float sum_left = 0;
+    int cnt_left = 0;
+    for (int i = bound_left; i <= 150; i++) {
+        if (valid[i]) {
+            sum_left += x_of(dist[i],i)-car_left;
+            cnt_left++;
+        } 
+    }
+    if (cnt_left) sum_left /= cnt_left;
+    float sum_right = 0; 
+    int cnt_right = 0;
+    for (int i = bound_right; i >= 30; i--) {
+        if (valid[i]) {
+            sum_right += x_of(dist[i],i)-car_right;
+            cnt_right++;
+        }
+    }
+    if (cnt_right) sum_right /= cnt_right;
     #ifdef Debug
-    Serial.printf("boundary found (%d, %d)! angle:(%f, %f)\r\n", 
+    Serial.printf("boundary found (%d, %d)! sum:(%f, %f), angle: %f\r\n", 
         bound_left, bound_right,
-        aleft, aright);
+        sum_left, sum_right, -0.15f*(sum_left+sum_right)+90);
     #endif
-    float angle;
-
-    if (aleft>aright) angle=aleft;
-    else if(aleft<aright) angle=-aright;
-    else angle=0;
-    return Go{speed, constrain(angle,-10,10)+90};
+    float angle=constrain(-12.f*(sum_left+sum_right)/min(abs(sum_right),abs(sum_left)), -10, 10);
+    // correction on speed and angle, based on distance
+        // from 0 to -angle.
+        int to=int(-angle*0.66)+90, count=0;
+        float sum=0;
+        if(angle<0) for(int i=90; i<to; i++) 
+            if(valid[i]) {sum+=dist[i]; count++;}
+        else for(int i=90; i>to; i--)
+            if(valid[i]) {sum+=dist[i]; count++;}
+        /// 100mm as danger zone
+        if(count>0){
+            sum/=count;
+            return Go{speed*constrain(sum*.01f, 0.8f, 1.f), angle*200/sum+90};
+        } else return Go{speed*0.5f, angle+90}; // ?
+    // go
+    return Go{speed, +90};
 }

freshman_cup/gogogo.h

@@ -3,6 +3,6 @@
 #define CUP_GO_H
 
 struct Go{float velocity, angle;};
-Go next();
+Go next(const float* dist, const bool* valid);
 
 #endif // CUP_GO_H