Diff: PsiSwarm/demo.cpp

Revision:

3:cd048f6e544e

Parent:

2:a6214fd156ff

Child:

5:598298aa4900

--- a/PsiSwarm/demo.cpp	Mon Oct 05 14:42:16 2015 +0000
+++ b/PsiSwarm/demo.cpp	Mon Oct 05 20:42:37 2015 +0000
@@ -17,6 +17,11 @@
 
 #include "psiswarm.h"
 
+// PID terms
+#define LF_P_TERM 0.2
+#define LF_I_TERM 0
+#define LF_D_TERM 4
+
 char top_menu = 0;
 char sub_menu = 0;
 char level = 0;
@@ -48,6 +53,15 @@
 char stress_step = 0;
 
 
+float lf_right;
+float lf_left;
+float lf_current_pos_of_line = 0.0;
+float lf_previous_pos_of_line = 0.0;
+float lf_derivative,lf_proportional,lf_integral = 0;
+float lf_power;
+float lf_speed = 0.4;
+
+
 
 void demo_mode()
 {
@@ -180,7 +194,7 @@
                                     if(side_ir_index == 0) side_ir_index = 7;
                                     else side_ir_index --;
                                 }
-                                if(sub_menu == 10) level = 0;
+                                if(sub_menu == 11) level = 0;
                                 break;
                             case 2: // Motor Menu
                                 if(sub_menu == 0) {
@@ -228,6 +242,15 @@
                                 if(sub_menu == 6) level = 0;
                                 break;
                             case 5: // Demo Menu
+                                if(sub_menu == 0) {
+                                    if(demo_running == 0) {
+                                        start_line_demo();
+                                    } else {
+                                        demo_running = 0;
+                                        display.set_backlight_brightness(bl_brightness * 0.01f);
+                                        stop();
+                                    }
+                                }
                                 if(sub_menu == 1) {
                                     if(demo_running == 0) {
                                         start_obstacle_demo();
@@ -332,7 +355,7 @@
                                     if(side_ir_index == 7) side_ir_index = 0;
                                     else side_ir_index ++;
                                 }
-                                if(sub_menu == 10) level = 0;
+                                if(sub_menu == 11) level = 0;
                                 break;
                             case 2: // Motor Menu
                                 if(sub_menu == 0) {
@@ -380,6 +403,15 @@
                                 if(sub_menu == 6) level = 0;
                                 break;
                             case 5: // Demo Menu
+                                if(sub_menu == 0) {
+                                    if(demo_running == 0) {
+                                        start_line_demo();
+                                    } else {
+                                        demo_running = 0;
+                                        display.set_backlight_brightness(bl_brightness * 0.01f);
+                                        stop();
+                                    }
+                                }
                                 if(sub_menu == 1) {
                                     if(demo_running == 0) {
                                         start_obstacle_demo();
@@ -427,7 +459,7 @@
                                 else sub_menu --;
                                 break;
                             case 1: //Sensors Menu
-                                if(sub_menu == 0) sub_menu = 10;
+                                if(sub_menu == 0) sub_menu = 11;
                                 else sub_menu --;
                                 break;
 
@@ -461,7 +493,7 @@
                                 else sub_menu ++;
                                 break;
                             case 1: //Sensors Menu
-                                if(sub_menu == 10) sub_menu = 0;
+                                if(sub_menu == 11) sub_menu = 0;
                                 else sub_menu ++;
                                 break;
                             case 2: //Motor Menu
@@ -503,7 +535,7 @@
                                 if(sub_menu == 13) level = 0;
                                 break;
                             case 1: // Sensors Menu
-                                if(sub_menu == 10) level = 0;
+                                if(sub_menu == 11) level = 0;
                                 break;
                             case 2: //Motor Menu
                                 if(sub_menu == 2) {
@@ -676,6 +708,11 @@
                             update_ultrasonic_measure();
                             break;
                         case 10:
+                            store_line_position();
+                            if(line_found == 1)sprintf(bottomline,"LINE:%1.3f",line_position);
+                            else sprintf(bottomline,"LINE:---------");
+                            break;
+                        case 11:
                             sprintf(bottomline,"EXIT");
                             break;
                     }
@@ -788,6 +825,17 @@
     }
 }
 
+void start_line_demo()
+{
+    display.set_backlight_brightness(0);
+    time_out = 0.25f;
+    demo_timer.start();
+    state = 0;
+    speed = 0;
+    led_step = 0;
+    demo_running = 1;
+    demo_timeout.attach_us(&line_demo_cycle,1000);
+}
 
 void start_obstacle_demo()
 {
@@ -830,6 +878,87 @@
     demo_timeout.attach_us(&spinning_demo_cycle,1000);
 }
 
+void line_demo_cycle()
+{
+    if(demo_timer.read() > time_out) {
+        store_line_position();
+        if(line_found) {
+            time_out = 0.01f;
+            state = 0;
+            // Get the position of the line.
+            lf_current_pos_of_line = line_position;
+            lf_proportional = lf_current_pos_of_line;
+
+            // Compute the derivative
+            lf_derivative = lf_current_pos_of_line - lf_previous_pos_of_line;
+
+            // Compute the integral
+            lf_integral += lf_proportional;
+
+            // Remember the last position.
+            lf_previous_pos_of_line = lf_current_pos_of_line;
+
+            // Compute the power
+            lf_power = (lf_proportional * (LF_P_TERM) ) + (lf_integral*(LF_I_TERM)) + (lf_derivative*(LF_D_TERM)) ;
+
+            // Compute new speeds
+            lf_right = lf_speed-lf_power;
+            lf_left  = lf_speed+lf_power;
+
+            // limit checks
+            if (lf_right < 0)
+                lf_right = 0;
+            else if (lf_right > 1.0f)
+                lf_right = 1.0f;
+
+            if (lf_left < 0)
+                lf_left = 0;
+            else if (lf_left > 1.0f)
+                lf_left = 1.0f;
+        }else{
+          //Cannot see line: hunt for it
+          if(lf_left > lf_right){
+            //Currently turning left, keep turning left   
+            state ++;
+            float d_step = state * 0.04;
+            lf_left = 0.2 + d_step;
+            lf_right = -0.2 - d_step;
+            if(state > 20){
+                state = 0;
+                lf_right = 0.2;
+                lf_left = -0.2;
+                time_out += 0.01f;
+                if(time_out > 0.1f) demo_running = 0;
+            }
+          }else{
+            //Currently turning right, keep turning right   
+            state ++;
+            float d_step = state * 0.04;
+            lf_left = -0.2 - d_step;
+            lf_right = 0.2 + d_step;
+            if(state > 20){
+                state = 0;
+                lf_right = -0.2;
+                lf_left = 0.2;
+                time_out += 0.01f;
+                if(time_out > 0.1f) demo_running = 0;
+            } 
+          }
+        }
+        // set speed
+        set_left_motor_speed(lf_left);
+        set_right_motor_speed(lf_right);
+
+
+        demo_timer.reset();
+    }
+    if(demo_running == 1)demo_timeout.attach_us(&line_demo_cycle,500);
+    else {
+        stop();
+        display.set_backlight_brightness(bl_brightness * 0.01f);
+    }
+}
+
 void stress_demo_cycle()
 {
     if(demo_timer.read() > time_out) {