diff options
Diffstat (limited to 'CODE/CODE.ino')
| -rw-r--r-- | CODE/CODE.ino | 186 |
1 files changed, 92 insertions, 94 deletions
diff --git a/CODE/CODE.ino b/CODE/CODE.ino index 0d225a4..f6918a7 100644 --- a/CODE/CODE.ino +++ b/CODE/CODE.ino @@ -66,6 +66,10 @@ int dpr6 = "LOW"; int dpr7 = "LOW"; int dpr8 = "LOW"; +double sp1 = 0.00; +double sp2 = 0.00; +double sp3 = 0.00; + void setup() { pinMode(m1enA, OUTPUT); pinMode(m1enB, OUTPUT); @@ -98,12 +102,10 @@ void setup() { Serial2.begin(9600); Timer3.initialize(800); // 800 microseconds / 1.25kHz - Timer3.attachInterrupt(updatePWM1); - Timer3.attachInterrupt(updatePWM2); - Timer3.attachInterrupt(updatePWM3); + Timer3.attachInterrupt(updatePWM); } -void updatePWM1() { +void updatePWM() { static int counter = 0; if (counter < PWMval1) { @@ -111,32 +113,14 @@ void updatePWM1() { } else { digitalWrite(m1enA, LOW); } - - counter++; - if (counter >= 100) { - counter = 0; - } -} - -void updatePWM2() { - static int counter = 0; - - if (counter < PWMval1) { + + if (counter < PWMval2) { digitalWrite(m2enA, HIGH); } else { digitalWrite(m2enA, LOW); } - - counter++; - if (counter >= 100) { - counter = 0; - } -} - -void updatePWM3() { - static int counter = 0; - - if (counter < PWMval1) { + + if (counter < PWMval3) { digitalWrite(m3enA, HIGH); } else { digitalWrite(m3enA, LOW); @@ -152,28 +136,76 @@ void mcpsoftpwm1(int value1) { if (value1 < 0) value1 = 0; if (value1 > 100) value1 = 100; - //Serial.print("softPWM pin: 3, value: "); - //Serial.println(value1); + Serial.print("softPWM pin: enA, value: "); + Serial.println(value1); PWMval1 = value1; } void mcpsoftpwm2(int value2) { if (value2 < 0) value2 = 0; if (value2 > 100) value2 = 100; - //Serial.print("softPWM pin: 3, value: "); - //Serial.println(value2); + Serial.print("softPWM pin: enA, value: "); + Serial.println(value2); PWMval2 = value2; -} -void mcpsoftpwm3(int value3) { +}void mcpsoftpwm3(int value3) { if (value3 < 0) value3 = 0; if (value3 > 100) value3 = 100; - //Serial.print("softPWM pin: 3, value: "); - //Serial.println(value3); + Serial.print("softPWM pin: enA, value: "); + Serial.println(value3); PWMval3 = value3; } -void drive(int speed, int direction){ +void move1(int sped1) { + sp1 = abs(sped1)*0.1; + mcpsoftpwm1(sp1); + digitalWrite(m1enB, LOW); + if (sped1 < 0) { + Serial.print("back "); + digitalWrite(m1pwm1, LOW); + digitalWrite(m1pwm2, HIGH); + } else { + Serial.print("forward "); + digitalWrite(m1pwm1, HIGH); + digitalWrite(m1pwm2, LOW); + } + Serial.println(sp1); +} + +void move2(int sped2) { + sp2 = abs(sped2)*0.1; + mcpsoftpwm2(sp2); + digitalWrite(m2enB, LOW); + if (sped2 < 0) { + Serial.print("back "); + digitalWrite(m2pwm1, LOW); + digitalWrite(m2pwm2, HIGH); + } else { + Serial.print("forward "); + digitalWrite(m2pwm1, HIGH); + digitalWrite(m2pwm2, LOW); + } + Serial.println(sp2); +} + +void move3(int sped3) { + sp3 = abs(sped3)*0.1; + mcpsoftpwm3(sp3); + digitalWrite(m3enB, LOW); + if (sped3 < 0) { + Serial.print("back "); + digitalWrite(m3pwm1, LOW); + digitalWrite(m3pwm2, HIGH); + } else { + Serial.print("forward "); + digitalWrite(m3pwm1, HIGH); + digitalWrite(m3pwm2, LOW); + } + Serial.println(sp3); +} + + +void drive(int speeeed, int direction){ // // Direction............|.Motor 1.|.Motor 2.|.Motor 3. // 0deg(forward)........|....0....|....-....|....+.... @@ -183,76 +215,42 @@ void drive(int speed, int direction){ // 240 deg..............|....-....|....+....|....0.... // 300 deg..............|....-....|....0....|....+.... // - - mcpsoftpwm1(speed); //pwm speed signal - mcpsoftpwm2(speed); //pwm speed signal - mcpsoftpwm2(speed); //pwm speed signal - digitalWrite(m1enB, LOW); - digitalWrite(m2enB, LOW); - digitalWrite(m3enB, LOW); - //Serial.println("debug2"); - //Serial.println(direction); - if(direction>330 || direction<=30){ + if(direction>330 || direction<=30){ //0 deg - digitalWrite(m1pwm1, LOW); - digitalWrite(m1pwm2, HIGH); - - digitalWrite(m2pwm1, LOW); - digitalWrite(m2pwm2, LOW); - - digitalWrite(m3pwm1, LOW); - digitalWrite(m3pwm2, HIGH); - //Serial.println("0 deg"); + //Serial.println("drive 0 deg"); + move1(0); + move2(-speeeed); + move3(speeeed); }else if (direction>30 && direction <= 90){ //60 deg - //Serial.println("60deg"); - digitalWrite(m1pwm1, LOW); - digitalWrite(m1pwm2, LOW); - - digitalWrite(m2pwm1, HIGH); - digitalWrite(m2pwm2, LOW); - - digitalWrite(m3pwm1, LOW); - digitalWrite(m3pwm2, HIGH); + //Serial.println("drive 60deg"); + move1(speeeed); + move2(-speeeed); + move3(0); }else if (direction>90 && direction <= 150){ //120 deg - //Serial.println("120 deg"); - digitalWrite(m1pwm1, HIGH); - digitalWrite(m1pwm2, LOW); - digitalWrite(m2pwm1, HIGH); - digitalWrite(m2pwm2, LOW); - digitalWrite(m3pwm1, LOW); - digitalWrite(m3pwm2, LOW); + //Serial.println("drive 120 deg"); + move1(speeeed); + move2(0); + move3(-speeeed); }else if (direction > 150 && direction <= 210) { //180 degrees - //Serial.println("on 180"); - digitalWrite(m1pwm1, HIGH); - digitalWrite(m1pwm2, LOW); - - digitalWrite(m2pwm1, LOW); - digitalWrite(m2pwm2, LOW); - - digitalWrite(m3pwm1, HIGH); - digitalWrite(m3pwm2, LOW); + //Serial.println("drive 180 deg"); + move1(0); + move2(speeeed); + move3(-speeeed); }else if (direction > 210 && direction <= 270) { //240 degrees - digitalWrite(m1pwm1, LOW); - digitalWrite(m1pwm2, LOW); - - digitalWrite(m2pwm1, LOW); - digitalWrite(m2pwm2, HIGH); - - digitalWrite(m3pwm1, HIGH); - digitalWrite(m3pwm2, LOW); + //Serial.println("drive 240 deg"); + move1(-speeeed); + move2(speeeed); + move3(0); }else if (direction > 270 && direction <= 330) { //300 degrees - digitalWrite(m1pwm1, LOW); - digitalWrite(m1pwm2, HIGH); - digitalWrite(m2pwm1, LOW); - digitalWrite(m2pwm2, HIGH); - //Serial.println("drive 300deg"); - digitalWrite(m3pwm1, LOW); - digitalWrite(m3pwm2, LOW); + //Serial.println("drive 300 deg"); + move1(-speeeed); + move2(0); + move3(speeeed); }else{ Serial.println("how?"); } |