File content as of revision 12:14bac44e33b9:

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

//INCLUDES
#include "mbed.h"
#include "stdio.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 "PinAssignment.h"
#include "LCD.h"
#include "Misc.h"
#include <string>

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

//Initialize Global Variables
I2C i2c(PIN_SDA,PIN_SCL);
Anemometer ane; //
MotorDriver motor;
LowPowerTimer t,t_mode, t_disp;
int mode = OP_CALIBRATION;

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
int main()
{
i2c.frequency(I2C_FREQ);
Accelerometer acc(&i2c); //Accelerometer
LCD lcd(&i2c);

float ang_P,ang_R;
float ref_R1,ref_R2;
int t_elapsed;
int wthres = WIND_THRES_INIT;
char buffer[16];

//FLAGS
int flag_time = 1; //Normal mode time
int flag_idle = 0; //Idling time
int flag_disp = 1; //Anti-flickering
int flag_bres = 0; //Flag for checking button released

//PUSH BUTTONS
Pushbutton bt_fn(PIN_BTFN,&mode,&flag_disp);
Pushbutton bt_inc(PIN_BTINC);
Pushbutton bt_dec(PIN_BTDEC);

string topL = "INITIALIZING";
string botL = "";
lcd.LCD_display(topL, botL);

t.start(); //Start timer
t_disp.start();
wait_us(1000000);

topL = "PUT SENSORS IN";
botL = "CALIBRATION SLOT";
lcd.LCD_display(topL, botL);

while(1)
{
    switch(mode)
    {
        case OP_PLACEMENT:{
            if(flag_disp){
                topL = "CALIBRATING";
                botL = "";
                lcd.LCD_display(topL,botL);
                acc.calibrate();
                topL = "PUT SENSOR ON";
                botL = "PANEL&REFLECTORS";
                lcd.LCD_display(topL,botL);
                flag_disp = 0;
            }
            
            break;
        }
////////////////////////////////////////////////////////////////////////////////
        case OP_NORMAL:{
            ane.checkWind(&mode);
            topL = "NORMAL:IDLING";
            if(timer_read_ms(t_disp) >= LCD_WRATE){
                botL = Misc::itos((int)ane.getWind(&flag_disp)) + " kph";
                t_disp.reset();
                t_disp.start();
            }
            if(flag_disp){
                lcd.LCD_display(topL,botL);
                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;
        }
////////////////////////////////////////////////////////////////////////////////
        case OP_WIND:{
            ane.checkWind(&mode);
            topL = "WIND SAFETY";
            if(timer_read_ms(t_disp) >= LCD_WRATE){
                botL = Misc::itos((int)ane.getWind(&flag_disp)) + " kph";
                t_disp.reset();
                t_disp.start();
            }
            if(flag_disp){
                lcd.LCD_display(topL,botL);
                flag_disp = 0;
            }
            
            //Move all motor backward
            motor.moveBackward(M_ALL);
            flag_time = 1; //Set the system in motion once windspeed has subsided
            break;
            
            flag_idle = 0;
            break;
        }
////////////////////////////////////////////////////////////////////////////////
        case OP_MANUAL1:{
            topL = "MANUAL: M1";
            botL = "";
            if(flag_disp){
                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(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;
        }
////////////////////////////////////////////////////////////////////////////////
        case OP_MANUAL2:{
            topL = "MANUAL: M2";
            botL = "";
            if(flag_disp){
                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;
        }
////////////////////////////////////////////////////////////////////////////////
        case OP_WSETTING:{
            topL = "Threshold:";
            botL = Misc::itos(wthres) + " kph";
            if(flag_disp){
                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_WSETTING_TIMEOUT)
            {
                mode = OP_NORMAL;
                flag_disp = 1;
                break;
            }
            ////////////////////////////////////////////////////////////////////
            
            if(!(flag_bres == 1 && bt_inc.read()) && !(flag_bres == -1 && bt_dec.read())){
                if(bt_inc.read() && wthres < WIND_THRES_MAX)
                {
                    ane.setThres(++wthres);
                    botL = Misc::itos(wthres) + " kph";
                    lcd.LCD_display(topL,botL);
                    flag_idle = 0;
                    flag_bres = 1;
                }
                else if(bt_dec.read() && wthres > WIND_THRES_MIN)
                {
                    ane.setThres(--wthres);
                    botL = Misc::itos(wthres) + " kph";
                    lcd.LCD_display(topL,botL);
                    flag_idle = 0;
                    flag_bres = -1;
                }
                else{
                    flag_bres = 0;
                }
            }
            
            flag_time = 1; //Set the system in motion once done adjusting
            
            break;
        }
    }
    wait_us(LOOP_DELAY);
}

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