Remote inc coolant nozzle control program
Dependencies: mbed reScale USBDevice
Dependents: Nozzle_inputs Nozzle_display
main.cpp
- Committer:
- BETZtechnik
- Date:
- 2019-03-29
- Revision:
- 18:40499bfc9ab2
- Parent:
- 8:6bb07a893f53
- Child:
- 19:0c2e51da8789
File content as of revision 18: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; } } // ***************************************************************************** } }