Mark and Peter
Remote inc coolant nozzle control program
Dependencies: mbed reScale USBDevice
Dependents: Nozzle_inputs Nozzle_display
main.cpp
- Committer:
- BETZtechnik
- Date:
- 2019-04-07
- Revision:
- 19:0c2e51da8789
- Parent:
- 18:40499bfc9ab2
File content as of revision 19:0c2e51da8789:
#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 = 20; // 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 = 500;
int minServo = 2500;
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); TESTING ONLY
int main() {
cw.mode(PullUp);
ccw.mode(PullUp);
aux1.mode(PullUp);
//aux1.rise(&triggered);
//servo2 = 0;
uint8_t c = 0; // for 485 link
// tw.start(); /// start twitch timer
while(1) {
//************************************ Interrupt response **********************
// if (tw.read() > 1200){ // timer can only hold 30 min, so reset after 20
//tw.reset();
//}
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;
//servo2=1;
}
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);
//servo2= 1;
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);
//servo2.pulsewidth_us(servo1Pos); // TESTING 1POS on Servo 2!!!!
}
if (twitch > 0){
if (twitchStart == 0){
// tw.start(); // start timer for nozzle sweep MOVED TO MAIN
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;
}
}
// *****************************************************************************
}
}