Diff: main.cpp

Revision:

12:14bac44e33b9

Parent:

11:2ade1ad7bad6

Child:

14:3989d03a8b98

--- a/main.cpp	Fri Mar 12 02:48:29 2021 +0000
+++ b/main.cpp	Fri Mar 12 03:10:20 2021 +0000
@@ -86,7 +86,7 @@
 ////////////////////////////////////////////////////////////////////////////////
         case OP_NORMAL:{
             ane.checkWind(&mode);
-            topL = "NORMAL";
+            topL = "NORMAL:IDLING";
             if(timer_read_ms(t_disp) >= LCD_WRATE){
                 botL = Misc::itos((int)ane.getWind(&flag_disp)) + " kph";
                 t_disp.reset();
@@ -97,6 +97,73 @@
                 flag_disp = 0;
             }
             
+            if(flag_time) //If delay interval has passed
+            {
+                ane.checkWind(&mode);
+                //Get Angle of Panel
+                ang_P = acc.getAngle(S_PANEL);
+                
+                //Calculate the Angle of Both Reflector
+                ref_R1 = Algorithm::calcAngle(1,ang_P);
+                ref_R2 = Algorithm::calcAngle(2,ang_P);
+                
+                //Moving Reflector 1
+                topL = "NORMAL:MOVING R1";
+                if(timer_read_ms(t_disp) >= LCD_WRATE){
+                    botL = Misc::itos((int)ane.getWind(&flag_disp)) + " kph";
+                    t_disp.reset();
+                    t_disp.start();
+                }
+                lcd.LCD_display(topL,botL);
+                
+                ang_R = acc.getAngle(S_R1);
+                while(ang_R <= ref_R1 && acc.checkAngle(ref_R1,ang_R))
+                {
+                    ane.checkWind(&mode);
+                    if(mode != OP_NORMAL) {break;}
+                    motor.moveForward(M1);
+                    ang_R = acc.getAngle(S_R1);
+                }
+                while(ang_R >= ref_R1 && acc.checkAngle(ref_R1,ang_R))
+                {
+                    ane.checkWind(&mode);
+                    if(mode != OP_NORMAL) {break;}
+                    motor.moveBackward(M1);
+                    ang_R = acc.getAngle(S_R1);
+                }
+                motor.stop();
+                
+                //Moving Reflector 2
+                topL = "NORMAL:MOVING R1";
+                if(timer_read_ms(t_disp) >= LCD_WRATE){
+                    botL = Misc::itos((int)ane.getWind(&flag_disp)) + " kph";
+                    t_disp.reset();
+                    t_disp.start();
+                }
+                lcd.LCD_display(topL,botL);
+                    
+                ang_R = acc.getAngle(S_R2);
+                while(ang_R <= ref_R2 && acc.checkAngle(ref_R2,ang_R))
+                {
+                    ane.checkWind(&mode);
+                    if(mode != OP_NORMAL) {break;}
+                    motor.moveForward(M2);
+                    ang_R = acc.getAngle(S_R2);
+                }
+                while(ang_R >= ref_R2 && acc.checkAngle(ref_R2,ang_R))
+                {
+                    ane.checkWind(&mode);
+                    if(mode != OP_NORMAL) {break;}
+                    motor.moveBackward(M2);
+                    ang_R = acc.getAngle(S_R2);
+                }
+                motor.stop();
+                flag_time = 0; //Reset timer flag
+                flag_disp = 1; //Reset display flag
+                t.reset(); //Reset timer
+            }
+            t_elapsed = (int)timer_read_s(t);
+            flag_time = (t_elapsed >= TIME_NORMAL) ? 1 : 0; //Enable flag if delay interval has passed
             flag_idle = 0;
             break;
         }
@@ -131,6 +198,48 @@
                 flag_disp = 0;
             }
             
+            //TIMEOUT
+            ////////////////////////////////////////////////////////////////////
+            if(!flag_idle) //Check if button is not pressed
+            {
+                t_mode.reset();
+                t_mode.start();
+                flag_idle = 1; //Indicate idling
+            }
+            else if(timer_read_s(t_mode) > TIME_MANUAL_TIMEOUT)
+            {
+                mode = OP_NORMAL;
+                flag_disp = 1;
+                break;
+            }
+            ////////////////////////////////////////////////////////////////////
+            
+            flag_disp = 1;
+            while(bt_inc.read()) //Extend
+            {
+                if(flag_disp){
+                    botL = "EXTENDING";
+                    lcd.LCD_display(topL,botL);
+                }
+                flag_idle = 0;
+                flag_disp = 0;
+                motor.moveForward(M1);
+            }
+            while(bt_dec.read()) //Retract
+            {
+                if(flag_disp){
+                    botL = "RETRACTING";
+                    lcd.LCD_display(topL,botL);
+                }
+                flag_idle = 0;
+                flag_disp = 0;
+                motor.moveBackward(M1);
+            }
+            if(!bt_inc.read() && !bt_dec.read() && !flag_disp)
+            {
+                flag_disp = 1;
+                motor.stop();
+            }
             break;
         }
 ////////////////////////////////////////////////////////////////////////////////
@@ -141,7 +250,49 @@
                 lcd.LCD_display(topL,botL);
                 flag_disp = 0;
             }
-                        
+            
+            //TIMEOUT
+            ////////////////////////////////////////////////////////////////////
+            if(!flag_idle) //Check if button is not pressed
+            {
+                t_mode.reset();
+                t_mode.start();
+                flag_idle = 1; //Indicate idling
+            }
+            else if(timer_read_s(t_mode) > TIME_MANUAL_TIMEOUT)
+            {
+                mode = OP_NORMAL;
+                flag_disp = 1;
+                break;
+            }
+            ////////////////////////////////////////////////////////////////////
+            
+            flag_disp = 1;
+            while(bt_inc.read()) //Extend
+            {
+                if(flag_disp){
+                    botL = "EXTENDING";
+                    lcd.LCD_display(topL,botL);
+                }
+                flag_idle = 0;
+                flag_disp = 0;
+                motor.moveForward(M2);
+            }
+            while(bt_dec.read()) //Retract
+            {
+                if(flag_disp){
+                    botL = "RETRACTING";
+                    lcd.LCD_display(topL,botL);
+                }
+                flag_idle = 0;
+                flag_disp = 0;
+                motor.moveBackward(M2);
+            }
+            if(!bt_inc.read() && !bt_dec.read() && !flag_disp)
+            {
+                flag_disp = 1;
+                motor.stop();
+            }
             break;
         }
 ////////////////////////////////////////////////////////////////////////////////