File content as of revision 2:944511c6c55f:

/* 
For settings of system behaviour, see Defs_sett.h
For pin assignment list, see PinAssignment.h
*/

//INCLUDES
#include "mbed.h"
#include "string"     // std::string, std::to_string
#include "Accelerometer.h"
#include "Anemometer.h"
#include "Algorithm.h"
#include "MotorDriver.h"
#include "Defs_Sett.h"
#include "Pushbutton.h"
#include "LCD.h"

#define timer_read_s(x)     chrono::duration_cast<chrono::seconds>((x).elapsed_time()).count()

//Initialize Global Variables
Accelerometer acc; //Accelerometer
Anemometer ane; //
MotorDriver motor;
LCD lcd;
LowPowerTimer t,t_mode;
Ticker ti;
int mode = OP_CALLIBRATION;

Pushbutton bt_fn(PIN_BTFN,&mode);
Pushbutton bt_inc(PIN_BTINC);
Pushbutton bt_dec(PIN_BTDEC);

//Function Declarations
void checkWind();

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
int main()
{
float ang_P,ang_R;
float ref_R1,ref_R2;
int flag_time = 1, flag_idle = 0;
int t_elapsed;
int wthres = WIND_THRES_INIT;

ti.attach(&checkWind,TICK_WIND);
t.start(); //Start timer

while(1)
{
    switch(mode)
    {
        case OP_NORMAL:
            lcd.LCD_display("NORMAL","");
            if(flag_time) //If delay interval has passed
            {
                ti.attach(&checkWind,TICK_WIND); //Enable Wind Safety
                checkWind();
                //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
                ang_R = acc.getAngle(S_R1);
                while(ang_R <= ref_R1 && acc.checkAngle(ref_R1,ang_R))
                {
                    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))
                {
                    if(mode != OP_NORMAL) {break;}
                    motor.moveBackward(M1);
                    ang_R = acc.getAngle(S_R1);
                }
                motor.stop();
                
                //Moving Reflector 2
                ang_R = acc.getAngle(S_R2);
                while(ang_R <= ref_R2 && acc.checkAngle(ref_R2,ang_R))
                {
                    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))
                {
                    if(mode != OP_NORMAL) {break;}
                    motor.moveBackward(M2);
                    ang_R = acc.getAngle(S_R2);
                }
                motor.stop();
                flag_time = 0; //Reset timer 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
            break;
////////////////////////////////////////////////////////////////////////////////
        case OP_WIND:
            lcd.LCD_display("WIND SAFETY","");
            //Move all motor backward
            motor.moveBackward(M_ALL);
            flag_time = 1; //Set the system in motion once windspeed has subsided
            break;
////////////////////////////////////////////////////////////////////////////////
        case OP_MANUAL1:
            lcd.LCD_display("MOTOR 1","");
            ti.detach(); //Disable Wind Safety
            
            //TIMEOUT
            ////////////////////////////////////////////////////////////////////
            if(!bt_inc.read() && !bt_dec.read() && !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;
            }
            ////////////////////////////////////////////////////////////////////
            
            while(bt_inc.read()) //Extend
            {
                flag_idle = 0;
                motor.moveForward(M1);
            }
            while(bt_dec.read()) //Retract
            {
                flag_idle = 0;
                motor.moveBackward(M1);
            }
            if(!bt_inc.read() && !bt_dec.read())
            {
                motor.stop();
            }
            flag_time = 1; //Set the system in motion once done adjusting
            break;
////////////////////////////////////////////////////////////////////////////////
        case OP_MANUAL2:
            lcd.LCD_display("MOTOR 2","");
            ti.detach(); //Disable Wind Safety
            
            //TIMEOUT
            ////////////////////////////////////////////////////////////////////
            if(!bt_inc.read() && !bt_dec.read() && !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;
            }
            ////////////////////////////////////////////////////////////////////
            
            while(bt_inc.read()) //Extend
            {
                flag_idle = 0;
                motor.moveForward(M2);
            }
            while(bt_dec.read()) //Retract
            {
                flag_idle = 0;
                motor.moveBackward(M2);
            }
            if(!bt_inc.read() && !bt_dec.read())
            {
                motor.stop();
            }
            flag_time = 1; //Set the system in motion once done adjusting
            break;
////////////////////////////////////////////////////////////////////////////////
        case OP_WSETTING:
            lcd.LCD_display("Threshold:",to_string(wthres));
            motor.stop();
            ti.detach(); //Disable Wind Safety
            
            //TIMEOUT
            ////////////////////////////////////////////////////////////////////
            if(!bt_inc.read() && !bt_dec.read() && !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_WSETTING_TIMEOUT)
            {
                mode = OP_NORMAL;
                break;
            }
            ////////////////////////////////////////////////////////////////////
            
            if(bt_inc.read() && wthres < WIND_THRES_MAX)
            {
                ane.setThres(++wthres);
                lcd.LCD_display("Threshold:",to_string(wthres));
            }
            else if(bt_inc.read() && wthres > WIND_THRES_MIN)
            {
                ane.setThres(--wthres);
                lcd.LCD_display("Threshold:",to_string(wthres));
            }
            
            flag_time = 1; //Set the system in motion once done adjusting
            break;
    }
}

}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//FUNCTIONS
void checkWind()
{
    ane.checkWind(&mode);
}