Mark and Peter
Remote inc coolant nozzle control program
Dependencies: mbed reScale USBDevice
Dependents: Nozzle_inputs Nozzle_display
Diff: main.cpp
- Revision:
- 21:db507362ca7a
- Parent:
- 20:8aebc19d9d1d
- Child:
- 22:a3ada529b264
diff -r 8aebc19d9d1d -r db507362ca7a main.cpp
--- a/main.cpp Wed Apr 10 20:25:04 2019 +0000
+++ b/main.cpp Wed Apr 10 23:23:13 2019 +0000
@@ -23,8 +23,9 @@
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(45,0,minServo,maxServo); //19850 is 1.0v from voltage divider at 10k.
PwmOut servo1(P1_13); //display board
@@ -36,6 +37,12 @@
int Ones = 0;
int twitch = 0;
+ int twitchStart=0;
+ int twitchDirection=0;
+ int lastTwitchMove = 0;
+ int maxTwitch = 0;
+ int minTwitch = 0;
+ int tReadMs = 50;
int currentNozzle = 0;
@@ -44,6 +51,7 @@
int n1Pos = 0;
int servo1Pos = 0;
+ int lastServo1Pos = 999;
DigitalOut servo2(P2_2);
@@ -65,7 +73,7 @@
*/
-
+Timer t;
int main() {
@@ -73,7 +81,7 @@
ccw.mode(PullUp);
aux1.mode(PullUp);
-
+t.start();
//aux1.rise(&triggered);
@@ -87,8 +95,12 @@
uint8_t c = 0; // for 485 link
while(1) {
+
-
+ if (t.read() > 1200){
+ t.reset();
+ lastTwitchMove = t.read_ms();
+ }
//************************************ Interrupt response **********************
@@ -131,178 +143,6 @@
//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 OLD ***************************
-
-/*
-while (display.readable())
-{
-
-
-
- c = display.getc();
-
- if (c == 'N'){
-
- index = 1;
-
- }
-
- if (index == 1){
- c = display.getc();
- if (c == '1'){ // N1
- currentNozzle = 1;
- index = 3;
-
-
- }
- else if (c == '2'){ //N2
- currentNozzle = 2;
- 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;
- }
-
-
- }
-
- 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;
- }
-
- else if (c=='0'){
- Ones = 0;
- }
-
-
-
-n1Pos = ((Tens * 10) + Ones);
-
-if ((n1Pos == 45) && (currentNozzle==1)){
-// servo2=1;
-}
-
-servo1Pos = servo1Scale.from(n1Pos);
-
-servo1.pulsewidth_us(servo1Pos);
-
-index=0;
-
-
-}
-}
-*/
-
//********************** Display code NEW with W **********************************
@@ -327,6 +167,7 @@
}
if (c == '1') {
twitch = 1;
+ index = 0;
}
if (c == '2') {
twitch = 2;
@@ -477,22 +318,92 @@
Ones = 9;
}
- n1Pos = ((Tens * 10) + Ones);
-
- if ((n1Pos == 45) && (currentNozzle==1)) {
-// servo2=1;
- }
-
- servo1Pos = servo1Scale.from(n1Pos);
-
- servo1.pulsewidth_us(servo1Pos);
-
- 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 (twitch == 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) {
+ 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 + 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) && (t.read_ms() > (lastTwitchMove + tReadMs))) {
+ 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 = t.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 = t.read_ms();
+ }
+ if (servo1Pos != lastServo1Pos) {
+ servo1.pulsewidth_us(servo1Pos);
+ lastServo1Pos = servo1Pos;
+ }
+ }
+
+}
//********************************* Inputs serial *******************************