File content as of revision 6:40499bfc9ab2:

 #include "mbed.h"

#include "reScale.h"


//Serial pc(USBTX, USBRX);

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

Timer tw;

DigitalIn aux1(P0_7);
DigitalIn cw(P1_24); 
DigitalIn ccw(P2_7);

int cwRequested = 0;
int ccwRequested = 0;

int aux1State = 0;

  int twitch = 0;
  int lastTwitch = 300;
  int lastTwitchMove = 0;
  int twitchStart = 0; // has twitching begun?
  int twitchDirection = 0; // 0 is up, 1 is down.
  int maxTwitch = 0;
  int minTwitch = 0;

int tw1 = 5; // stages of servo write to control speed of sweep. 11.11 Ms = 1 degree
int tw2 = 10;
int tw3 = 15;

int twReadMs = 10; // interval of twitch changes us.


DigitalOut dir(P0_15);

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



// reScale servo1Scale(45,0,750,1800); //19850 is 1.0v from voltage divider at 10k.

int maxServo = 2500;
int minServo = 500;
reScale servo1Scale(-90,90,minServo, maxServo);
reScale servo2Scale(-90,90,minServo, maxServo);

 PwmOut servo1(P1_13); //Servo 1
 PwmOut servo2(P2_2); //Servo 2
 
//char tens = '0';
//char ones = '1';

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

int index = 0;               // for parsing serial

int n1Pos = 0;
int lastN1Pos = 300;
int n2Pos = 0;
int lastN2Pos = 300;

  int servo1Pos = 0;
  int lastServo1Pos = 10;
int servo2Pos =0;
int lastServo2Pos = 0;
//DigitalOut servo2(P2_2);

           
    


int main() {
    
cw.mode(PullUp);
ccw.mode(PullUp);
aux1.mode(PullUp);




//aux1.rise(&triggered);



//servo2 = 0;



      uint8_t c = 0; // for 485 link
  
    while(1) {
        
//************************************  Interrupt response **********************       
       
        
        if (aux1 == 0 && aux1State == 0)
        {
           
           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(.001);
}

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

if (aux1 == 1 && aux1State == 1)
{
aux1State = 0;
//wait(0.1);
}          
            
        /*
if (cwRequested == 1)
{
dir= 1;
//wait(0.001);
display.printf("M");
wait(0.01);
display.printf("+");
wait(0.001);
dir=0;
cwRequested = 0;
//wait(.001);
}


else if (ccwRequested == 1)
{
dir= 1;
//wait(0.01);
display.printf("M");
wait(0.01);
display.printf("-");
wait(0.001);
dir=0;
ccwRequested = 0;
//wait(.001);
}
   */ 
    
        
//**********************************  Display serial ***************************       

while (display.readable())
{
             

                
               c = display.getc();
               
                if (c == 'N'){
                    
                    index = 1;
                      
                   }
                   
                   if (c == 'W'){  // wiggle setting
                       index = 5;
                       }
                       
if (index == 5){
c = display.getc();   
if (c == '0'){
 twitch = 0;
 index = 0;
 }
if (c == '1'){
 twitch = 1;
 }
 if (c == '2'){
 twitch = 2;
 index = 0;
 }
 if (c == '3'){
 twitch = 3;
 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;
                        }
                     if (c == '2'){
                        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;
                     index = 0;
                     
                     }
                     
                     else if (c=='1'){
                         Ones = 1;
                        index = 0;
                          
                         }
                         
                         else if (c=='2'){
                             Ones = 2;
                           index = 0;
                             }
                             
                             else if (c=='3'){
                                 Ones = 3;
                                index = 0;
                                 }
                                 
                                 else if (c=='4'){
                                     Ones = 4;
                                     index = 0;
                                     }
                                     
                                     else if (c=='5'){
                                     Ones = 5;
                                     index = 0;
                                     }
                                     
                                     else if (c=='6'){
                                     Ones = 6;
                                     index = 0;
                                     }
                                     
                                     else if (c=='7'){
                                     Ones = 7;
                                     index = 0;
                                     }
                                     
                                     else if (c=='8'){
                                     Ones = 8;
                                     index = 0;
                                     }
                                     
                                     else if (c=='9'){
                                     Ones = 9;
                                     index = 0;
                                     }
                                     

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

//servo1.pulsewidth_us(servo1Pos);              
                         
index=0;
}

 if (currentNozzle == 2){                                     
n2Pos = (((Hundreds *100) + (Tens *10) + Ones) - 90);
twitchStart = 0; // trigger sweep from new angle                       
//servo2Pos = servo1Scale.from(n2Pos);

//servo2.pulsewidth_us(servo2Pos);              
                         
index=0;
}
                
}


                 
}


if (twitch == 0){ 
tw.stop();
twitchStart = 0;
twitchDirection = 0;
servo1Pos = servo1Scale.from(n1Pos);
servo1.pulsewidth_us(servo1Pos); 
servo2Pos = servo2Scale.from(n2Pos);
servo2.pulsewidth_us(servo2Pos);
}

if (twitch > 0){
    if (twitchStart == 0){
    tw.start();  // start timer for nozzle sweep
    servo1Pos = servo1Scale.from(n1Pos);
    servo1.pulsewidth_us(servo1Pos); 
    
    lastTwitchMove = tw.read_ms(); // store time of last nozzle movement
    minTwitch = servo1Pos; // store original servo postion for bottom of sweep
    if (twitch == 1){
    maxTwitch = servo1Pos + 150; // 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 + 200; 
    }
    if (twitch == 3){
    maxTwitch = servo1Pos + 250; 
    }      
    twitchStart = 1;
}
    if ((twitchStart == 1) && (tw.read_ms() > (lastTwitchMove + twReadMs))){
        if (twitchDirection == 0){ //going up
        servo1Pos = servo1Pos + 25; // add variable amount of uS to the servo signal
                
        if (servo1Pos > maxServo){ 
            servo1Pos = maxServo;
            twitchDirection = 1; //reverse direction
            }
         if (servo1Pos > maxTwitch){
             servo1Pos = maxTwitch;
             twitchDirection = 1; //reverse direction
             }
            lastTwitchMove = tw.read_ms();
            }
            
        if (twitchDirection == 1){ // going down    
               servo1Pos = servo1Pos - 25; // add variable amount of Ms to the servo signal
        if (servo1Pos < minServo){ 
            servo1Pos = minServo;
            twitchDirection = 0; //reverse direction
            }
         if (servo1Pos < minTwitch){
             servo1Pos = minTwitch;
             twitchDirection = 0; //reverse direction
             }
            lastTwitchMove = tw.read_ms();
            }
if (servo1Pos != lastServo1Pos){
            servo1.pulsewidth_us(servo1Pos); 
            lastServo1Pos = servo1Pos;
            }         
}    
}

//********************************* 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;
                      
                      }
                      
                      
                        }


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



}
}