File content as of revision 0:bf14022d00d5:

#include "mbed.h"
#include "I2C.h"
#include "SLCD.h"

// v12
// partea de control merge

// intrarile de la controlerul SMC sunt inversate

#define NO_POWER_SUPPLY      0x7F
#define LECP6_IN_ALARM       0x60
#define RETURN_TO_ORIGIN     0x02
#define LECP6_SCAN_INPUTS    0x07
#define LECP6_MOVES          0x08
#define LECP6_DONE           0x09
#define LECP6_SETUP          0x0A
#define LECP6_JOG            0x15
#define POSITION_OVERFLOW    0x11
#define POSITION_UNDERFLOW   0x12
#define POSITION_RECORDED    0x30
#define ESC                  0x40
#define BUILD_CTRL           0x50
#define CTRL_CREATED         0x51
#define CTRL_RUN                 0x52
#define LECP6_RST            0xFF


Ticker operating_led;
Ticker equipment_status;

// 4 digit LCD
SLCD slcd;
/*
I2C I2C_Comm(PTC9,PTC8);
I2C I2C_LCD(PTC2,PTC1);
const int addr = 0x90;
//Serial UART_Comm(PTA2, PTA1);
*/
DigitalOut LED(PTE29);
DigitalOut Relay_CTRL(PTE31);
DigitalOut HP_Output(PTB19);
DigitalIn Board_But_Left(PTC12);
DigitalIn Board_But_Right(PTC3);

// MCU Digital Output to control Axis/Gripper
/*
DigitalOut DO_A3_IN0(PTE18);
DigitalOut DO_A4_IN1(PTE17);
DigitalOut DO_A5_IN2(PTE16);
DigitalOut DO_A6_IN3(PTE6);
DigitalOut DO_A7_IN4(PTE3);
DigitalOut DO_A8_IN5(PTE2);
*/
DigitalOut DO_A9_SETUP(PTA14);
DigitalOut DO_A10_HOLD(PTA6);
DigitalOut DO_A11_DRIVE(PTA7);
DigitalOut DO_A12_RESET(PTC16);
DigitalOut DO_A13_SVON(PTC13);

// MCU Digital Input for feedback from Axis/Gripper
DigitalIn DI_B1_OUT0(PTE19);
DigitalIn DI_B2_OUT1(PTE1);
DigitalIn DI_B3_OUT2(PTD7);
DigitalIn DI_B4_OUT3(PTA17);
DigitalIn DI_B5_OUT4(PTA16);
DigitalIn DI_B6_OUT5(PTA15);
DigitalIn DI_B7_BUSY(PTA13);
DigitalIn DI_B8_AREA(PTD2);
DigitalIn DI_B9_SETON(PTD4);
DigitalIn DI_B10_INP(PTD6);
DigitalIn DI_B11_SVRE(PTD7);
DigitalIn DI_B12_ESTOP(PTD5);
DigitalIn DI_B13_ALARM(PTE0);

BusOut step_number(PTE18,PTE17,PTE16,PTE6,PTE3,PTE2);
BusIn equipment_active_step(PTE19, PTE1, PTD7, PTA17, PTA16, PTA15);
BusIn equipment_system_status(PTA13, PTD2, PTD4, PTD6, PTD7, PTD5, PTE0);

DigitalIn OPEN_CLOSE(PTB2);

/*
I2C ioLCD(PTC1,PTC2);
DigitalIn HP_Input(PTB18);
*/

DigitalIn BUT_Verde(PTE30);
DigitalIn BUT_Maro(PTB20);
DigitalIn BUT_Portocaliu(PTE23);
DigitalIn BUT_Rosu(PTB2);

/*
DigitalOut DO_1(PTA12);
DigitalOut DO_2(PTA4);
*/

void config();
void operating_flag();
void LECP6_read_inputs();
void axis_position();
void showIndex_Inc(int);
void showMessage(int);

bool LECP_alarms();
void LECP_alarms_rst();
bool LECP6_SVRE();
bool LECP6_BUSY();
bool LECP_move_axis();
bool LECP_return_to_origin();
void LECP6_disable_outputs();
void LECP_Jog();
void LECP_Build_Control();
void LECP_Run_Control();
void LECP_Run(int*);


bool equipment_ready = false, equip_in_alarm = false;

int sys_Status, equip_active;
int move_forward = 0, move_backward = 0;
float memorised_positions[10];
float control_sequences[10][10];
int number_of_mem_positions = 0;
float position;
float positions[] = { 0.0, 0.1, 0.25, 0.5, 1.0, 2.5, 5.0, 10.0, 15.0, 20.0, 25.0, 30.0, 35.0, 40.0, 45.0, 50.0, 55.0, 60.0, 65.0, 70.0, 75.0, 80.0, 85.0, 90.0, 95.0, 100.0, 110.0, 120.0, 130.0, 140.0, 150.0,
                      -0.1,-0.25,-0.5,-1.0,-2.5,-5.0,-10.0,-15.0,-20.0,-25.0,-30.0,-35.0,-40.0,-45.0,-50.0,-55.0,-60.0,-65.0,-70.0,-75.0,-80.0,-85.0,-90.0,-95.0,-100.0,-110.0,-120.0,-130.0,-140.0,-150.0
                    };

int main()
{
    config();

    control_sequences[1][0] = 3;
    control_sequences[1][1] = 20.0;
    control_sequences[1][2] = 50.0;
    control_sequences[1][3] = 25.0;
    /*      int contr[10];
            contr[0] = 4;
            contr[1] = 5;
            contr[2] = 28;
            contr[3] = 58;
            contr[4] = 35;
    */
    //LECP_return_to_origin();
    //LECP_Run(contr);

    while(1) {

        /******* RETURN TO ORIGIN ********/
        if(sys_Status != NO_POWER_SUPPLY) {
            if(!equip_in_alarm && (!equipment_ready || !Board_But_Left)) {
                showMessage(RETURN_TO_ORIGIN);
                equipment_ready = LECP_return_to_origin();
            }
        }

        /********* JOGING ***********/
        if(equipment_ready && !BUT_Portocaliu) {
            showMessage(LECP6_JOG);
            LECP_Jog();
        }

        /******** CONTROL ***********/
        if(equipment_ready && !BUT_Maro) {
            showMessage(BUILD_CTRL);
            LECP_Build_Control();
        }

        /******** RUN CONTROL SEQUENCE ********/
        if(equipment_ready && !BUT_Verde) {
            showMessage(CTRL_RUN);
            LECP_Run_Control();
        }
    }
}



void LECP_Run(int* ctrl)
{

    // in functia aceasta sa introduc si return to origin

    int move_axis_task = 0, alarm = 0;
    int last_step = ctrl[0], step;

    while(BUT_Rosu && !equip_in_alarm && equipment_ready && !alarm) { // cat timp nu este oprita secventa de control
        switch(move_axis_task) {
            case 0:
                // check if axis is not in fault or alarm
                while(equip_in_alarm) {
                }
                move_axis_task++;   // next task
                break;

            case 1:
                DO_A13_SVON = true; // turn servo on;
                if(LECP6_SVRE()) {
                    move_axis_task++;   // next task
                } else {
                    alarm = 1;
                    move_axis_task = 100;
                }
                break;

            case 2:
                for( step = 1; step < last_step && !equip_in_alarm; step++) {
                    step_number = ctrl[step];
                    if(step_number == 0 ) position = 0;
                    else position = position + positions[step_number];
                    DO_A11_DRIVE = true; // set drive bit
                    slcd.clear();
                    slcd.Home();
                    wait_ms(50);
                    DO_A11_DRIVE = false; // reset drive bit
                    slcd.printf("%.1f", position);
                    do {
                        wait_ms(10);
                    } while(DI_B10_INP);
                    do {
                        wait_ms(10);
                    } while(LECP6_BUSY());

                }
                if(!equip_in_alarm) move_axis_task = 0;   // next task
                else {
                    alarm = 2;
                    move_axis_task = 100;
                }
                break;

            default:
                break;
        }
    }
    DO_A13_SVON = false; // turn servo off;

}

/*** BUILD CONTROL SEQUENCES BASES ON MEMORIZED POSITIONS ***/
void LECP_Build_Control()
{
    bool control_seq_finished = false, position_memorized = false, delay_selected = false;
    int selected_memorized_position = 0, ctrl_sequence = 0, pos_sequence = 0, delay_value = 0;
    float sel_mem_pos_value = 0.0;

    // add a new control sequence
    control_sequences[0][0]++;

    while(!control_seq_finished && equipment_ready) {

        if(!BUT_Portocaliu) { // swap thrgough memorized positions
            position_memorized = 0;
            while(!position_memorized) {
                // show actual memorized position
                sel_mem_pos_value = memorised_positions[selected_memorized_position];
                // clear display
                slcd.clear();
                // go home display
                slcd.Home();
                // disable dots
                slcd.DP3(false);
                slcd.DP2(false);
                // print value
                slcd.printf("%.1f", sel_mem_pos_value);
                wait(1.0);

                // select another position from positions list
                if(!Board_But_Right && selected_memorized_position < number_of_mem_positions ) {
                    selected_memorized_position++;
                }
                if(!Board_But_Left && selected_memorized_position > 0 ) {
                    selected_memorized_position--;
                }

                // memorize in the control sequence the selected position from available ones
                if(!BUT_Verde) {
                    ctrl_sequence = control_sequences[0][0];
                    control_sequences[ctrl_sequence][++pos_sequence] = sel_mem_pos_value;
                    showMessage(POSITION_RECORDED);
                    position_memorized = true;
                }

                // cancel
                if(!BUT_Rosu) {
                    position_memorized = true;
                }
            }
            wait(1.0);
            // goto
        }

        // selectare delay
        if(!BUT_Maro) {

            delay_selected = false;
            while(!delay_selected) {
                // clear display & go home
                slcd.clear();
                slcd.Home();

                // disable dots
                slcd.DP3(false);
                slcd.DP2(false);
                // print value
                slcd.printf("%d", delay_value);

                if(!Board_But_Right && delay_value < 5 ) {
                    delay_value++;
                }
                if(!Board_But_Left && delay_value > 0 )  {
                    delay_value--;
                }

                if(!BUT_Verde) {
                    // memorize in the control sequence the selected position from available ones
                    ctrl_sequence = control_sequences[0][0];
                    control_sequences[ctrl_sequence][++pos_sequence] = 999.0;  // delay special identification code
                    control_sequences[ctrl_sequence][++pos_sequence] = delay_value;
                    showMessage(POSITION_RECORDED);
                    delay_selected = true;
                }

                // cancel
                if(!BUT_Rosu) {
                    delay_selected = true;
                }
                wait(1.0);
            }
        }

        // finish control sequence
        if(!BUT_Verde) {
            control_seq_finished = true;
            // save number of positions in the control sequence
            control_sequences[ctrl_sequence][0] = pos_sequence;
            showMessage(CTRL_CREATED);
        }

        // cancel control sequence
        if(!BUT_Rosu) {
            showMessage(ESC);
            control_seq_finished = true;
            for(int i = 0; i < 10; i++) {
                control_sequences[ctrl_sequence][i] = 0;
            }
            control_sequences[0][0]--;
        }
    }
}

/*** Run control sequence ***/
void LECP_Run_Control()
{
    // decompozitie pozitii in pozitiile momorate in controler si in delay-uri;

    int operations[10] = {0}, index, i, j;
    float decompose;

    if(position != 0.0) {
        LECP_return_to_origin();
    }

    while(BUT_Rosu) { // cat timp nu este oprita secventa de control
        // decompozitie doar o singura data va trebui rulata
        index = 1;
        for( i = 1; i <= control_sequences[1][0]; i++) {

            //decompose = control_sequences[1][i]-position;
            if(i>1) {
                decompose = control_sequences[1][i]-control_sequences[1][i-1];
            } else decompose = control_sequences[1][i]-position;

            if(decompose > 0 && decompose != 999.0 ) {
                while(decompose) {
                    for( j = 1; positions[j] < decompose && j <= 30; j++) {
                    }
                    if(decompose > 0) decompose -= positions[j];
                    operations[index++] = j;
                }

            }
            if(decompose < 0) {
                while(decompose) {
                    for( j = 31; positions[j] > decompose && j <= 60; j++) {
                    }
                    if(decompose < 0) decompose = decompose + ( positions[j] * -1.0);
                    operations[index++] = j;
                }
            }
            if(decompose == 999.0) {
                wait(control_sequences[1][i++]);
            }
            /*
                        for(int k = 0; k < index; k++) {
                            if(previous <= control_sequences[1][i]) {
                                move_forward = operations[k]; // nu e prea bine dar va merge
                            } else {
                                move_backward = operations[k] + 30;
                            }
                            LECP_move_axis();
                        }
            */
        }
        // add the absolute 0 position - pentru a nu avea offset la prima pozitie pentru dupa primul ciclu
        operations[index++] = 0;
        operations[0] = index;
        LECP_Run(operations);
    }
}


/*** SHOW INDEX and CORESPONDING INCREMENT WHILE JOGING***/
void showIndex_Inc(int index)
{
    slcd.DP3(false);
    slcd.DP2(false);
    slcd.DP1(false);

    slcd.clear();
    slcd.Home();
    slcd.printf(" %d",index); // print actual position index
    wait(0.5);
    slcd.clear();
    slcd.Home();
    slcd.printf("%.1f", positions[index]); // print position increment coresponding to the actual position index
    wait(0.5);
}

/*** Display Axis Calculated Poistion ***/
void axis_position()
{
    slcd.clear();
    slcd.Home();

    if(position < 10) {
        slcd.CharPosition = 2;
    }
    if(position < 100 && position > 10) {
        slcd.CharPosition = 1;
    }
    if(position >= 100) {
        slcd.CharPosition = 0;
    }

    slcd.DP3(false);
    slcd.DP2(false);
    slcd.printf("%.1f", position);
    wait(0.5);
    if(position > 155.0 || position < 0.0 ) {
        slcd.printf("A.L.M. .", position);
        wait(1.0);
    }
}

/*** MOVE COMMAND ***/
bool LECP_move_axis()
{
    int move_axis_task = 0, alarm = 0;
    bool move_axis_done = 0;

    // load step data
    if(move_forward) step_number = move_forward;
    else step_number = move_backward;

    move_forward = move_backward = 0;

    showMessage(LECP6_SCAN_INPUTS);

    do {
        switch(move_axis_task) {
            case 0:
                // if not in fault or alarm
                DO_A13_SVON = true; // turn servo on;
                move_axis_task++;   // next task
                wait(0.25);
                break;

            case 1: // check if SVRE bit is on
                if(LECP6_SVRE()) {
                    DO_A11_DRIVE = true; // set drive bit
                    showMessage(LECP6_MOVES);

                    DO_A11_DRIVE = false; // reset drive bit

                    move_axis_task++; // next task
                } else {
                    alarm = 1;
                    move_axis_task = 100;
                }

                break;

            case 2:
                if(!LECP6_BUSY()) { // check to see if LECP6 is busy for
                    move_axis_task++; // next task
                } else {
                    alarm = 2;
                    move_axis_task = 100;
                }
                break;

            case 3:
                do {
                    wait(0.25);
                } while(DI_B10_INP);
                DO_A13_SVON = false;
                showMessage(LECP6_DONE);

                move_axis_done = true;
                move_axis_task++;
                break;

            default:
                break;
        }
    } while(!move_axis_done || alarm);

    if(alarm) {
        LECP6_disable_outputs();
        return 0;
    }

    position += positions[step_number]; // calculate new position
    axis_position();
    return 1;
}


/*** Configure Smart Axis ***/
void config()
{
    LECP6_disable_outputs();

    // configure RED LED to toglle every 1.0 second
    operating_led.attach(&operating_flag,1.0);
    equipment_status.attach(&LECP6_read_inputs, 0.25);

    wait(2.0);
}

/*** read inputs from LCPE ***/
void LECP6_read_inputs()
{
    sys_Status = equipment_system_status;
    equip_active = equipment_active_step;

    // check if there is power to CTRL unit of SMC axis
    if(sys_Status == NO_POWER_SUPPLY) {
        showMessage(NO_POWER_SUPPLY);
        equipment_ready = 0;
    }

    // check for alarms
    if(equip_in_alarm && !Board_But_Left && !Board_But_Right) { // if alarms and both FRDM-kl46z are pressed reset alarms
        showMessage(LECP6_RST);
        LECP_alarms_rst();
        equip_in_alarm = LECP_alarms();
    } else { // show alarm message and raise flag
        if(sys_Status != NO_POWER_SUPPLY && (sys_Status & LECP6_IN_ALARM)) {
            showMessage(LECP6_IN_ALARM);
            equip_in_alarm = true;
        }
    }
}

/********************************************testate****************************************************/

/*** Disable outputs from uC to LCPE ***/
void LECP6_disable_outputs()
{
    DO_A9_SETUP = DO_A10_HOLD = DO_A11_DRIVE = DO_A12_RESET = DO_A13_SVON = false;
    step_number = 0;
}

/*** 1 second operating led ***/
void operating_flag()
{
    LED = !LED;
}

void showMessage(int message)
{
    slcd.clear();
    slcd.Home();

    if(message == POSITION_OVERFLOW) {
        slcd.printf("oF  ");
    }
    if(message == POSITION_UNDERFLOW) {
        slcd.printf("uF  ");
    }
    if(message == POSITION_RECORDED) {
        slcd.printf("R.E.C. ");
    }
    if(message == ESC) {
        slcd.printf("e.s.c. ");
    }
    if(message == RETURN_TO_ORIGIN) {
        slcd.printf("o.r.i.g.");
    }
    if(message == NO_POWER_SUPPLY) {
        slcd.printf("n.o.P.S");
    }
    if(message == LECP6_SCAN_INPUTS) {
        slcd.printf("s.c.a.n");
    }
    if(message == LECP6_MOVES) {
        slcd.printf("m.o.v.e");
    }
    if(message == LECP6_DONE) {
        slcd.printf("d.o.n.e");
    }
    if(message == LECP6_SETUP) {
        slcd.printf("s.e.t. ");
    }
    if(message == LECP6_JOG) {
        slcd.printf("j.o.g. ");
    }
    if(message == BUILD_CTRL) {
        slcd.printf("C.t.r.N");
    }
    if(message == CTRL_CREATED) {
        slcd.printf("C.t.r.S");
    }
    if(message == CTRL_RUN) {
        slcd.printf("r.u.n. ");
    }
    if(message == LECP6_IN_ALARM) {
        slcd.printf("A.L.r. ");
    }
    if(message == LECP6_RST) {
        slcd.printf("R.S.T. ");
    }
    wait(1.0);
}


/*** RETURN TO ORIGIN COMMAND ***/
bool LECP_return_to_origin()
{
    int task = 0, alarm = 0;
    bool ret_to_orig_done = false;

    do {
        switch(task) {
            case 0:
                while(equip_in_alarm) {
                }
                task++;
                break;

            case 1: // turn servo on

                DO_A13_SVON = true;
                if(LECP6_SVRE()) task++;
                else {
                    alarm = 2;
                    task = 99;
                }
                break;

            case 2:

                DO_A9_SETUP = true; // set drive bit
                showMessage(LECP6_SETUP);

                while(DI_B9_SETON) {
                    wait(0.1);
                }
                while(LECP6_BUSY()) {
                }

                DO_A9_SETUP = false; // reset drive bit
                task++;
                break;

            case 3:

                DO_A13_SVON = false;
                showMessage(LECP6_DONE);
                ret_to_orig_done = true;
                position = 0.0;
                task++;
                break;

            default:
                break;
        }
    } while(!ret_to_orig_done && !alarm);

    if(alarm) {
        LECP6_disable_outputs();
        return 0; // issues were identified while return to origin procedure
    }
    axis_position();
    return 1; // succesfully returned to origin
}

/*** FREE JOG WITH AXIS & POSITION RECORD***/
void LECP_Jog()
{
    bool jog_finished = false;
    int position_index = 1;

    while(!jog_finished && equipment_ready && !equip_in_alarm) {

        showIndex_Inc(position_index);
        axis_position();

        if(!Board_But_Right) { // move forward if actual position + position coresponding to the actual increment does result in a axis position overflow
            if((position + positions[position_index]) <= 150) {
                move_forward = position_index;
            } else {
                showMessage(POSITION_OVERFLOW);
            }
        }

        if(!Board_But_Left) { // move backward if actual position - position coresponding to the actual increment does result in a axis position underflow
            if((position + positions[position_index+30]) >= 0) {
                move_backward = position_index + 30;
            } else {
                showMessage(POSITION_OVERFLOW);
            }
        }

        if(!BUT_Portocaliu && position_index < 31) {
            position_index++;    // increase actual increment
        }
        if(!BUT_Maro && position_index > 0) {
            position_index--;    // decrease actual increment
        }

        if(!BUT_Verde) { // record axis position
            number_of_mem_positions++;
            memorised_positions[number_of_mem_positions] = position;
            showMessage(POSITION_RECORDED);
        }

        if(!BUT_Rosu) { // end free axis jog
            showMessage(ESC);
            jog_finished = true;
        }

        if(move_forward || move_backward) { // move axis forward or backward accordingly to the coresponding actual index position
            LECP_move_axis();
        }
    }
}

/*** CHECK FOR LCEP ALARMS ***/
bool LECP_alarms()
{
    // check for alarms
    if(DI_B12_ESTOP || DI_B13_ALARM) return 1;
    else return 0;
}

/*** RESET LCEP ALARMS ***/
void LECP_alarms_rst()
{
    DO_A12_RESET = true;
    wait(0.5);
    DO_A12_RESET = false;
}

/*** CHECK IF SVRE BIT BECAME ACTIVE AFTER SERVO ON CMD ***/
bool LECP6_SVRE()
{
// check if SVRE bit is on as an action to Servo ON
    int timeout = 0;
    do {
        //wait(0.1);
        wait_ms(50);
        timeout++;
        if(timeout >= 10) return 0;
    } while(DI_B11_SVRE);
    return 1;
}

/*** CHECK IF LCPE IS BUSY PERFORMING AN OPERATION ***/
bool LECP6_BUSY()
{
    int timeout = 0;
    do {
        wait(0.1);
        timeout++;
        if(timeout >= 10) return 1;
    } while(!DI_B7_BUSY);
    return 0;
}