File content as of revision 29:44d5454ce8fa:

#include "mbed.h"
#include "Process.h"
#include "Pulse.h"
#include <stdlib.h>
#include "../../CommonLibraries/PID/PID.h"
#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
#include "../../Communication/RS485/LineHub/LineHub.h"
#include "../../Communication/Controller/Controller.h"
#include "../../Input/Switch/Switch.h"

#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"

using namespace SWITCH;
using namespace PID_SPACE;

using namespace LINEHUB;


static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;

static bool lock;
static bool processChangeComp;
static int current;

static void AllActuatorReset();

#ifdef USE_SUBPROCESS
static void (*Process[USE_PROCESS_NUM])(void);
#endif

#pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE

/*Replace here with the definition code of your variables.*/


Serial pc(USBTX, USBRX);

//**************Buzzer****************
//DigitalOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
bool EMGflag = false;
PwmOut buzzer(BUZZER_PIN);
//**************Buzzer****************

//************TapeLed*****************
void TapeLedEms_func();
TapeLedData tapeLED;
TapeLedData sendLedData;
TapeLED_Mode ledMode = Normal;
Ticker tapeLedTimer;
//************TapaLed*****************

const int omni[15][15] = {
    {    0,     5,    21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255 },
    {   -5,     0,     5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255 },
    {  -21,    -5,     0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255 },
    {  -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255 },
    {  -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255 },
    { -130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255 },
    { -187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255 },
    { -255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255 },
    { -255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187 },
    { -255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130 },
    { -255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83 },
    { -255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47 },
    { -255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21 },
    { -255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5 },
    { -255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0 }
};

const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };


uint8_t SetStatus(int);
uint8_t SetPWM(int);

#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE

#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0(void);
#endif
#if USE_PROCESS_NUM>1
static void Process1(void);
#endif
#if USE_PROCESS_NUM>2
static void Process2(void);
#endif
#if USE_PROCESS_NUM>3
static void Process3(void);
#endif
#if USE_PROCESS_NUM>4
static void Process4(void);
#endif
#if USE_PROCESS_NUM>5
static void Process5(void);
#endif
#if USE_PROCESS_NUM>6
static void Process6(void);
#endif
#if USE_PROCESS_NUM>7
static void Process7(void);
#endif
#if USE_PROCESS_NUM>8
static void Process8(void);
#endif
#if USE_PROCESS_NUM>9
static void Process9(void);
#endif
#endif

void SystemProcessInitialize()
{
    #pragma region USER-DEFINED_VARIABLE_INIT
    /*Replace here with the initialization code of your variables.*/

    #pragma endregion USER-DEFINED_VARIABLE_INIT

    lock = true;
    processChangeComp = true;
    current = DEFAULT_PROCESS;

#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
    Process[0] = Process0;
#endif
#if USE_PROCESS_NUM>1
    Process[1] = Process1;
#endif
#if USE_PROCESS_NUM>2
    Process[2] = Process2;
#endif
#if USE_PROCESS_NUM>3
    Process[3] = Process3;
#endif
#if USE_PROCESS_NUM>4
    Process[4] = Process4;
#endif
#if USE_PROCESS_NUM>5
    Process[5] = Process5;
#endif
#if USE_PROCESS_NUM>6
    Process[6] = Process6;
#endif
#if USE_PROCESS_NUM>7
    Process[7] = Process7;
#endif
#if USE_PROCESS_NUM>8
    Process[8] = Process8;
#endif
#if USE_PROCESS_NUM>9
    Process[9] = Process9;
#endif
#endif
}

static void SystemProcessUpdate()
{
#ifdef USE_SUBPROCESS
    if(controller->Button.HOME) lock = false;

    if(controller->Button.START && processChangeComp) {
        current++;
        if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
        processChangeComp = false;
    } else if(controller->Button.SELECT && processChangeComp) {
        current--;
        if (current < 0) current = 0;
        processChangeComp = false;
    } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
#endif

#ifdef USE_MOTOR
    ACTUATORHUB::MOTOR::Motor::Update(motor);
#endif

#ifdef USE_SOLENOID
    ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
#endif

#ifdef USE_RS485
    ACTUATORHUB::ActuatorHub::Update();
    //LINEHUB::LineHub::Update();
#endif

}


void SystemProcess()
{
    SystemProcessInitialize();
    while(1) {
        
        buzzer.period(1.0/800);

#ifdef USE_MU
        controller = CONTROLLER::Controller::GetData();
#endif

#ifdef USE_ERRORCHECK
        if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
            CONTROLLER::Controller::DataReset();
            AllActuatorReset();
            lock = true;
        } else
#endif
        {

#ifdef USE_SUBPROCESS
            if(!lock) {
                Process[current]();
            } else
#endif
            {
                //ロック時の処理
            }
        }


        //Emergency!
        /*
        if(!EMG_0 && !EMG_1 && !EMGflag){
        	buzzer = 0;
        	BuzzerTimer.attach(BuzzerTimer_func, 1);
        	EMGflag = true;
        	LED_DEBUG0 = 1;
        }
        if(EMG_0 && EMG_1 && EMGflag){
        	buzzer = 1;
        	BuzzerTimer.detach();
        	EMGflag = false;
        }
        */
        SystemProcessUpdate();
    }
}




#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0()
{
    AllActuatorReset();
}
#endif

#if USE_PROCESS_NUM>1
static void Process1()
{

}
#endif

#if USE_PROCESS_NUM>2
static void Process2()
{

}
#endif

#if USE_PROCESS_NUM>3
static void Process3()
{
    
}
#endif

#if USE_PROCESS_NUM>4
static void Process4()
{
    
}
#endif

#if USE_PROCESS_NUM>5
static void Process5()
{

}
#endif

#if USE_PROCESS_NUM>6
static void Process6()
{

}
#endif

#if USE_PROCESS_NUM>7
static void Process7()
{
   
}
#endif

#if USE_PROCESS_NUM>8
static void Process8()
{
	
}
#endif

#if USE_PROCESS_NUM>9
static void Process9()
{

}
#endif
#endif
#pragma endregion PROCESS

static void AllActuatorReset()
{
	#ifdef USE_SOLENOID
	    solenoid.all = ALL_SOLENOID_OFF;
	#endif
	
	#ifdef USE_MOTOR
	    for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
	        motor[i].dir = FREE;
	        motor[i].pwm = 0;
	    }
	#endif
}

void BuzzerTimer_func()
{
    buzzer = !buzzer;
    //LED_DEBUG0 = !LED_DEBUG0;
}

void TapeLedEms_func()
{
    sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}

#pragma region USER-DEFINED-FUNCTIONS

uint8_t SetStatus(int pwmVal)
{
    if (pwmVal < 0) return BACK;
    else if (pwmVal > 0) return FOR;
    else if (pwmVal == 0) return BRAKE;
    else return BRAKE;
}

uint8_t SetPWM(int pwmVal)
{
    if (pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
    else return abs(pwmVal);
}

#pragma endregion