File content as of revision 23:baedb9e32b6d:

#include "mbed.h"

#include "reScale.h"

// 2019-10-01: Updated to include pulse, Nozzle direction.

//Serial pc(USBTX, USBRX);

//InterruptIn aux1(P0_7, PullUp); // index pulse

DigitalIn aux1(P0_7);
DigitalIn aux2(P1_21);
DigitalIn cw(P1_24);
DigitalIn ccw(P2_7);
DigitalIn pulse(P0_6);

int cwRequested = 0;
int ccwRequested = 0;

int pulseActive = 0;
int pulseFlag =1; // is pulse high or low. (off is high after the opto)
int pulseStart = 0;
int pulseCount = 0;
int lastPulse = 0; // for debounce
int pulseDebounce = 1; //ms for debounce pulse signal

int aux1State = 0;

int autoMode = 0; // setting for AUTO mode (X7a, Pulse etc.)
int aux2Mode =0; // aux 2 use, twitch wait, etc.
int twitchWaitFlag = 0; // was twitch wait enabled?
int waitFlag = 0;

DigitalOut dir(P0_15);
DigitalOut auxOut1(P0_8);

Serial display(P0_14, P0_13, 19200);  // RS 485  TX RX?
Serial inputs(P1_23,P0_20, 9600); //rs232 TX RX


int maxServo = 2500;
int minServo = 500;
reScale servo1Scale(90,-90,minServo,maxServo); //19850 is 1.0v from voltage divider at 10k.

PwmOut servo1(P1_13); // PRODUCTION
//PwmOut servo1(P2_2);  // TEST!!!


//char tens = '0';
//char ones = '1';

int Hundreds = 0;
int Tens = 0;
int Ones = 0;

int twitch = 0;
int twitchStart=0;
int twitchDirection=0;
int lastTwitchMove = 0;
int maxTwitch1 = 0;
int maxTwitch2 = 0;
int minTwitch1 = 0;
int minTwitch2 = 0;
int tReadMs = 5;
int twitchIncrement = 10;

int currentNozzle = 0;

int index = 0;               // for parsing serial

int n1Pos = 0;

int lastN1Called = 0;  // store the last actual tool position sent from the display because twitch code can shift it.

int n1Dir = 0;

int servo1Pos = 0;
int lastServo1Pos = 0;


//DigitalOut servo2(P2_18);

/*
void triggered()
{
    servo2 = 1;

    if (cw == 0){
        cwRequested = 1;
        }

        else if (ccw == 0){
            ccwRequested = 1;
            }

}

*/

Timer t;

int main()
{

    servo1.period(0.020);          // servo requires a 20ms period

    cw.mode(PullUp);
    ccw.mode(PullUp);
    aux1.mode(PullUp);
    aux2.mode(PullUp);
    pulse.mode(PullUp);

    // auxOut1 = 1;

    t.start();
    lastPulse = t.read_ms();

//aux1.rise(&triggered);



//servo2 = 0;



    uint8_t c = 0; // for 485 link

    while(1) {

        if (t.read()  > 1200) {
            t.reset();
            lastTwitchMove = t.read_ms();
            lastPulse = t.read_ms();
        }


//************************************  Interrupt response **********************


        if (aux1 == 0 && aux1State == 0) {

            wait(0.1);     //**************************2019-06-27

            if (cw == 1) {
//servo2 = 1;
                dir= 1;
//wait(0.001);
                display.printf("M");
                wait(0.05);
                display.printf("-");
                wait(0.001);
                dir=0;
                aux1State = 1;
//wait(.1);
            }

            if (ccw == 1) {
                dir= 1;
//wait(0.01);
                display.printf("M");
                wait(0.05);
                display.printf("+");
                wait(0.001);
                dir=0;
                aux1State = 1;
//wait(.1);
            }
        }

        if (aux1 == 1 && aux1State == 1) {
            aux1State = 0;
//wait(0.1);
        }

//********************** Tool Pulse **********************************

        /*
        if (pulse == 0)
        {
            pulseCount = 103;
                  dir=1;
                display.printf("T");
                    wait(0.05);
                    display.printf("%d", pulseCount);
                    wait(0.001);
                    dir=0;
                    wait(0.2);
            }
        */

        /*
        if (pulse == 0 && pulseActive == 0)
        {
        pulseStart = t.read_ms();
        pulseActive = 1;
        pulseCount = 1;
        wait(0.01);
        }

        if (pulse == 0 && pulseActive == 1 && (t.read_ms() < pulseStart + 1000))
        {
        pulseCount = pulseCount + 1;
        wait(0.01);
        }

        if (pulseActive == 1 && (t.read_ms() > pulseStart + 1000))
        {
        pulseCount = pulseCount + 100;

                dir=1;
                display.printf("T");
                    wait(0.05);
                    display.printf("%d", pulseCount);
                    wait(0.001);
                    dir=0;

                pulseCount = 0;
                pulseActive = 0;

                    wait(0.01);

        }
         */


        if (pulse == 0 && pulseActive == 0 && pulseFlag == 1 && (t.read_ms() > (lastPulse + pulseDebounce))) {
            pulseStart = t.read_ms();
            lastPulse = t.read_ms();
            pulseActive = 1;
            pulseCount = 1;
            pulseFlag = 0; // pin is now low
//wait(0.01);
        }

        if (pulse == 1 && pulseActive == 1 && pulseFlag == 0 && (t.read_ms() > lastPulse + pulseDebounce)) {
            pulseFlag = 1;
            lastPulse = t.read_ms();
        }

        if (pulse == 0 && pulseActive == 1 && pulseFlag == 1 && (t.read_ms() > lastPulse + pulseDebounce)) {
            pulseCount = pulseCount + 1;
            pulseFlag =0;
            lastPulse = t.read_ms();
//wait(0.01);
        }

        if (pulseActive == 1 && (t.read_ms() > pulseStart + 1000)) {
            pulseCount = pulseCount + 100;

            dir=1;
            display.printf("T");
            wait(0.05);
            display.printf("%d", pulseCount);
            wait(0.001);
            dir=0;

            pulseCount = 0;
            pulseActive = 0;
            pulseFlag=1;
            lastPulse = t.read_ms();
            //wait(0.01);
        }





//********************** Display code NEW with W **********************************

        while (display.readable()) {

            c = display.getc();

            if (c == 'N') {

                index = 1;


            }

            if (c == 'W') { // wiggle setting
                index = 5;
            }

            if (c == 'A') { // auto mode
                index = 8;
            }

            if (c == 'I') { // Input mode
                index = 9;
            }

            if (c == 'D') { // nozzle direction
                c = display.getc();
                if (c == '1') { // N1
                    index = 6;
                }
            }
            /*
            if (c == '2'){  // N2
             index = 7;
             }
            }
            */

            if (index == 5) {
                c = display.getc();
                if (c == '0') {
                    twitch = 0;
                    index = 0;
                }
                if (c == '1') {
                    twitch = 1;
                    index = 0;
                }
                if (c == '2') {
                    twitch = 2;
                    index = 0;
                }
                if (c == '3') {
                    twitch = 3;
                    index = 0;
                }
            }

            if (index == 6) {
                c = display.getc();
                if (c == '0') {
                    n1Dir = 0;
                    index = 0;
                }
                if (c == '1') {
                    n1Dir = 1;
                    index = 0;
                }
            }

            if (index == 8) { // auto mode
                c = display.getc();
                if (c == '1') {
                    autoMode = 1; // X7a (x7 atc)
                    index = 0;
                }
                if (c == '2') {
                    autoMode = 2;  // Pulse
                    index = 0;
                }

            }


            if (index == 9) { // aux2 mode
                c = display.getc();
                if (c == '0') {
                    aux2Mode = 0; // 0= off, 1 = twitch pause wait for coolant signal
                    index = 0;
                }
                if (c == '1') {
                    aux2Mode = 1;  // twitch pause
                    index = 0;
                }

            }



            if (index == 1) {       //Display is sending -90>+90 and adding 190 for N1 so would send 100 to 280 for -90 to +90
                c = display.getc();
                if (c == '1') {  // N1
                    currentNozzle = 1;
                    Hundreds = 0;
                    index = 3;


                }

                else if (c == '2') { //N2
                    currentNozzle = 1;
                    Hundreds = 1;
                    index = 3;
                }

                if (c == '3') { //N2     // Display adding 390 for N2 so 300 to 480 would be -90 to +90
                    currentNozzle = 2;
                    Hundreds = 0;
                    index = 3;
                }

                if (c == '4') { //N2
                    currentNozzle = 2;
                    Hundreds = 1;
                    index = 3;
                }


            }



            if (index == 3) {

                c = display.getc();

                if (c=='0') {
                    Tens = 0;
                    index = 4;

                }

                else if (c=='1') {
                    Tens = 1;
                    index = 4;

                }

                else if (c=='2') {
                    Tens = 2;
                    index = 4;

                }

                else if (c=='3') {
                    Tens = 3;
                    index = 4;

                }

                else if (c=='4') {
                    Tens = 4;
                    index = 4;
                }

                else if (c=='5') {
                    Tens = 5;
                    index = 4;
                }

                else if (c=='6') {
                    Tens = 6;
                    index = 4;
                }

                else if (c=='7') {
                    Tens = 7;
                    index = 4;
                }

                else if (c=='8') {
                    Tens = 8;
                    index = 4;
                }

                else if (c=='9') {
                    Tens = 9;
                    index = 4;
                }


            }



            if (index == 4) {

                c = display.getc();

                if (c=='0') {
                    Ones = 0;
                }

                else if (c=='1') {
                    Ones = 1;
                }

                else if (c=='2') {
                    Ones = 2;
                }

                else if (c=='3') {
                    Ones = 3;
                }

                else if (c=='4') {
                    Ones = 4;
                }

                else if (c=='5') {
                    Ones = 5;
                }

                else if (c=='6') {
                    Ones = 6;
                }

                else if (c=='7') {
                    Ones = 7;
                }

                else if (c=='8') {
                    Ones = 8;
                }

                else if (c=='9') {
                    Ones = 9;
                }



                if (currentNozzle == 1) {
                    n1Pos = (((Hundreds *100) + (Tens *10) + Ones) - 90);
                    twitchStart = 0; // trigger sweep from new angle
                    servo1Pos = servo1Scale.from(n1Pos);
                    lastN1Called = servo1Pos;
//servo1.pulsewidth_us(servo1Pos);
                    index=0;
                }




            }
        }

        /*

                    if (twitch == 0 || (twitch > 0 && aux2Mode == 1 && aux2 == 0)) {
                        if (servo1Pos != lastServo1Pos) {
                            twitchStart = 0;
                            twitchDirection = 0;
                            servo1Pos = servo1Scale.from(n1Pos);
                            servo1.pulsewidth_us(servo1Pos);
        //servo2Pos = servo2Scale.from(n2Pos);
        //servo2.pulsewidth_us(servo2Pos);
                            lastServo1Pos = servo1Pos;
                            if (twitch > 0 && aux2Mode == 1 && aux2 == 0 && last
                        }
                    }


                                if (twitch > 0) {
                                    if (twitchStart == 0) {
                                        //tw.start();  // start timer for nozzle sweep
                                        //servo1Pos = servo1Scale.from(n1Pos);
                                        servo1.pulsewidth_us(servo1Pos);
                                        lastTwitchMove = t.read_ms(); // store time of last nozzle movement
                                        minTwitch = servo1Pos; // store original servo postion for bottom of sweep
                                        if (twitch == 1) {
                                            maxTwitch = servo1Pos - 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
                                        }
                                        if (twitch == 2) {
                                            maxTwitch = servo1Pos - 150;
                                        }
                                        if (twitch == 3) {
                                            maxTwitch = servo1Pos - 200;
                                        }
                                        twitchStart = 1;

                                    }
                                    if ((twitchStart == 1) && (t.read_ms() > (lastTwitchMove + tReadMs))) {
                                        if (twitchDirection == 0) { //going up
                                            servo1Pos = servo1Pos - 10; // add variable amount of uS to the servo signal

                                            if (servo1Pos < minServo) {
                                                servo1Pos = minServo;
                                                twitchDirection = 1; //reverse direction
                                            }
                                            if (servo1Pos < maxTwitch) {
                                                servo1Pos = maxTwitch;
                                                twitchDirection = 1; //reverse direction
                                            }
                                            lastTwitchMove = t.read_ms();
                                        }

                                        if (twitchDirection == 1) { // going down
                                            servo1Pos = servo1Pos + 10; // add variable amount of Ms to the servo signal
                                            if (servo1Pos > maxServo) {
                                                servo1Pos = maxServo;
                                                twitchDirection = 0; //reverse direction
                                            }
                                            if (servo1Pos > minTwitch) {
                                                servo1Pos = minTwitch;
                                                twitchDirection = 0; //reverse direction
                                            }
                                            lastTwitchMove = t.read_ms();
                                        }
                                        if (servo1Pos != lastServo1Pos) {
                                            servo1.pulsewidth_us(servo1Pos);
                                            lastServo1Pos = servo1Pos;
                                        }
                                    }




                    }
                    */



        if ((twitch == 0) || (twitch > 0 && aux2Mode == 1 && aux2 == 1)) {
            if (servo1Pos != lastServo1Pos) {
                twitchStart = 0;
                twitchDirection = 0;
                servo1Pos = servo1Scale.from(n1Pos);
                servo1.pulsewidth_us(servo1Pos);
                lastServo1Pos = servo1Pos;
            }
        }

        /*
                    dir=1;
                    display.printf("X");
                    // wait(0.05);
                    // display.printf("1");
                    wait(0.05);
                    dir=0;
                    waitFlag = 1;
                    wait(0.5);
        */


        if (twitch > 0 && aux2Mode != 1 || (twitch > 0 && aux2Mode == 1 && aux2 == 0)) {
            if (twitchStart == 0) {
                //tw.start();  // start timer for nozzle sweep
                //servo1Pos = servo1Scale.from(n1Pos);
                servo1.pulsewidth_us(servo1Pos);
                lastTwitchMove = t.read_ms(); // store time of last nozzle movement
                minTwitch1 = lastN1Called; // store original servo postion for bottom of sweep




                if(n1Dir == 0) {
                    if (twitch == 1) {
                        maxTwitch1 = lastN1Called - 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
                    }
                    if (twitch == 2) {
                        maxTwitch1 = lastN1Called - 150;
                    }
                    if (twitch == 3) {
                        maxTwitch1 = lastN1Called - 200;
                    }
                }

                if(n1Dir == 1) {  // reverse
                    if (twitch == 1) {
                        maxTwitch1 = lastN1Called + 100; // 11.11mS per degree of sweep, 55 = 5 degrees of sweep. This should be a viariable set on the display.
                    }
                    if (twitch == 2) {
                        maxTwitch1 = lastN1Called + 150;
                    }
                    if (twitch == 3) {
                        maxTwitch1 = lastN1Called + 200;
                    }
                }

                twitchStart = 1;

            }


            if ((twitchStart == 1) && (t.read_ms() > (lastTwitchMove + tReadMs))) {
                if (twitchDirection == 0) { //going up
                    if (n1Dir ==0) {
                        servo1Pos = servo1Pos - twitchIncrement; // add variable amount of uS to the servo signal
                    }


                    if (n1Dir ==1) {
                        servo1Pos = servo1Pos + twitchIncrement; // add variable amount of uS to the servo signal
                    }


                    if (n1Dir ==0 && servo1Pos < maxTwitch1 ) {
                        servo1Pos = maxTwitch1;
                        twitchDirection = 1; //reverse direction
                    }

                    //      if (n2Dir == 0 && servo2Pos < maxTwitch2) {
                    //         servo2Pos = maxTwitch2;
                    //         twitchDirection = 1; //reverse direction
                    //    }
                    if (n1Dir ==1 && servo1Pos > maxTwitch1) {
                        servo1Pos = maxTwitch1;
                        twitchDirection = 1; //reverse direction
                    }

                    //     if (n2Dir == 1 && servo2Pos > maxTwitch2) {
                    //        servo2Pos = maxTwitch2;
                    //       twitchDirection = 1; //reverse direction
                    //   }


                    lastTwitchMove = t.read_ms();
                }

                if (twitchDirection == 1) { // going down

                    if (n1Dir ==0) {
                        servo1Pos = servo1Pos + twitchIncrement; // add variable amount of uS to the servo signal
                    }

                    if (n1Dir ==1) {
                        servo1Pos = servo1Pos - twitchIncrement; // add variable amount of uS to the servo signal
                    }

                    /*
                        if (n1Dir == 0 && servo1Pos < minServo) {
                            servo1Pos = minServo;
                            twitchDirection = 0; //reverse direction
                        }
                        */
                    if (n1Dir == 0 && servo1Pos > (minTwitch1+50)) {
                        servo1Pos = minTwitch1;
                        twitchDirection = 0; //reverse direction
                    }
                    if (n1Dir == 1 && servo1Pos < (minTwitch1 - 50)) {
                        servo1Pos = minTwitch1;
                        twitchDirection = 0; //reverse direction
                    }
                    //       if (n2Dir == 0 && servo2Pos > (minTwitch2+0)) {
                    //       servo2Pos = minTwitch2;
                    //       twitchDirection = 0; //reverse direction
                    //   }
                    //       if (n2Dir == 1 && servo2Pos < minTwitch2) {
                    //       servo2Pos = minTwitch2;
                    //       twitchDirection = 0; //reverse direction
                    //   }

                    lastTwitchMove = t.read_ms();
                }

                if (servo1Pos != lastServo1Pos) {
                    servo1.pulsewidth_us(servo1Pos);
                    lastServo1Pos = servo1Pos;
                }



            }




        }

        if (twitch >0 && aux2Mode == 1 && aux2 == 1) {
            if (waitFlag == 0) {
                dir=1;
                display.printf("X");
                wait(0.05);
                display.printf("X");
                wait(0.001);
                dir=0;
                waitFlag =1;
                // wait (0.1);
            }
        }
        if ((twitch == 0) || (twitch >0 && aux2Mode == 1 && aux2 == 0)|| (twitch >0 && aux2Mode == 0)) {
            if (waitFlag == 1) {
                dir=1;
                display.printf("Y");
                wait(0.05);
                display.printf("Y");
                wait(0.001);
                dir=0;
                waitFlag =0;
                // wait (0.1);
            }
        }




//********************************* Inputs serial *******************************


        while (inputs.readable()) {


            c = inputs.getc();

            if (c == 'T') {

                index = 1;
                //servo2=1;
            }


            if (index == 1) {
                c = inputs.getc();


                if (c=='0') {
                    Tens = 0;
                    //servo2=1;
                }

                else if (c=='1') {
                    Tens = 1;


                }

                else if (c=='2') {
                    Tens = 2;

                }

                else if (c=='3') {
                    Tens = 3;

                }

                else if (c=='4') {
                    Tens = 4;
                }

                else if (c=='5') {
                    Tens = 5;
                }

                else if (c=='6') {
                    Tens = 6;
                }

                else if (c=='7') {
                    Tens = 7;
                }

                else if (c=='8') {
                    Tens = 8;
                }

                else if (c=='9') {
                    Tens = 9;
                }

                else if (c=='0') {
                    Tens = 0;
                }
                index = 2;
            }


            if (index == 2) {
                c = inputs.getc();
                if (c=='0') {
                    Ones = 0;

                }

                else if (c=='1') {
                    Ones = 1;


                }

                else if (c=='2') {
                    Ones = 2;

                }

                else if (c=='3') {
                    Ones = 3;

                }

                else if (c=='4') {
                    Ones = 4;
                }

                else if (c=='5') {
                    Ones = 5;
                }

                else if (c=='6') {
                    Ones = 6;
                }

                else if (c=='7') {
                    Ones = 7;
                }

                else if (c=='8') {
                    Ones = 8;
                }

                else if (c=='9') {
                    Ones = 9;
                }

                else if (c=='0') {
                    Ones = 0;
                }
                int tempToolNo = (Tens*10)+100+Ones;
                dir=1;
                display.printf("T");
                wait(0.05);
                display.printf("%d", tempToolNo);
                wait(0.001);
                dir=0;
                index = 0;
                tempToolNo=0;

            }


        }


// *****************************************************************************



    }
}