Mark and Peter
Remote inc coolant nozzle control program
Dependencies: mbed reScale USBDevice
Dependents: Nozzle_inputs Nozzle_display
Diff: main.cpp
- Revision:
- 18:40499bfc9ab2
- Parent:
- 8:6bb07a893f53
- Child:
- 19:0c2e51da8789
--- a/main.cpp Tue Feb 19 05:56:53 2019 +0000
+++ b/main.cpp Fri Mar 29 06:16:09 2019 +0000
@@ -7,6 +7,8 @@
//InterruptIn aux1(P0_7, PullUp); // index pulse
+Timer tw;
+
DigitalIn aux1(P0_7);
DigitalIn cw(P1_24);
DigitalIn ccw(P2_7);
@@ -16,6 +18,20 @@
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);
@@ -24,13 +40,20 @@
-reScale servo1Scale(45,0,750,1800); //19850 is 1.0v from voltage divider at 10k.
+// reScale servo1Scale(45,0,750,1800); //19850 is 1.0v from voltage divider at 10k.
- PwmOut servo1(P1_13); //display board
+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;
@@ -39,29 +62,18 @@
int index = 0; // for parsing serial
int n1Pos = 0;
+int lastN1Pos = 300;
+int n2Pos = 0;
+int lastN2Pos = 300;
int servo1Pos = 0;
-
-
-DigitalOut servo2(P2_2);
+ int lastServo1Pos = 10;
+int servo2Pos =0;
+int lastServo2Pos = 0;
+//DigitalOut servo2(P2_2);
-/*
-void triggered()
-{
- servo2 = 1;
+
- if (cw == 0){
- cwRequested = 1;
- }
-
- else if (ccw == 0){
- ccwRequested = 1;
- }
-
-}
-
-*/
-
int main() {
@@ -77,7 +89,7 @@
-servo2 = 0;
+//servo2 = 0;
@@ -169,21 +181,56 @@
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){
+ 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;
}
- else if (c == '2'){ //N2
+ 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;
+ }
+
}
@@ -220,6 +267,32 @@
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;
+ }
+
+
}
@@ -230,71 +303,148 @@
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;
}
- else if (c=='0'){
- Ones = 0;
- }
+
-
-n1Pos = ((Tens * 10) + Ones);
-
-if ((n1Pos == 45) && (currentNozzle==1)){
-// servo2=1;
+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;
}
-
-servo1Pos = servo1Scale.from(n1Pos);
-servo1.pulsewidth_us(servo1Pos);
+ 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 *******************************
@@ -308,7 +458,7 @@
if (c == 'T'){
index = 1;
- servo2=1;
+ // servo2=1;
}
@@ -318,7 +468,7 @@
if (c=='0'){
Tens = 0;
- servo2=1;
+ //servo2=1;
}
else if (c=='1'){