soft PWM added

1 files changed, 128 insertions, 54 deletions

diff --git a/debug/motordriver/motordriver.ino b/debug/motordriver/motordriver.ino
index e98c6bc..2b159f2 100644
--- a/debug/motordriver/motordriver.ino
+++ b/debug/motordriver/motordriver.ino
@@ -1,25 +1,27 @@
-
 #include <Adafruit_MCP23X17.h>
+#include <TimerThree.h>
+
+
 //moveohmi = https://robotics.stackexchange.com/questions/7829/how-to-program-a-three-wheel-omni
 //https://www.pololu.com/product/2997
 //https://www.pololu.com/product/4861
 //
 // motor one/A  front
-#define  m1enA 3   // GPA3
+#define  m1enA 3   // GPA3  PWM1
 #define  m1enB 2   // GPA2
 #define  m1pwm1 0  // GPA0
 #define  m1pwm2 1  // GPA1
 #define  m1encA 12 // GPB4
 #define  m1encB 13 // GPB5
 // motor two/B  left
-#define  m2enA 7   // GPA7
+#define  m2enA 7   // GPA7  PWM2
 #define  m2enB 6   // GPA6
 #define  m2pwm1 4  // GPA4
 #define  m2pwm2 5  // GPA5
 #define  m2encA 14 // GPB6
 #define  m2encB 15 // GPB7
 // motor three/C    right
-#define  m3enA 11  // GPB3
+#define  m3enA 11  // GPB3  PWM3
 #define  m3enB 10  // GPB2
 #define  m3pwm1 8  // GPB0
 #define  m3pwm2 9  // GPB1
@@ -28,6 +30,7 @@
 
 double sp = 1.000; //speed in 0-1
 
+//kiwidrive
 double dirA = 0.00;
 double dirB = 0.00;
 double dirC = 0.00;
@@ -37,6 +40,7 @@ double spdC = 0.00;
 double dirrad = 0.00;
 double pi = 3.1415926535897;
 
+//encoder
 double Arpm = 0.00;
 volatile long Aencount = 0;
 unsigned long ALtime = 0;
@@ -53,46 +57,116 @@ unsigned long CLtime = 0;
 unsigned long CCtime = 0;
 unsigned long CPtime = 0;
 
+//softPWM
+volatile int PWMval1 = 0;   // PWM m1enA value 0-100
+volatile int PWMval2 = 0;   // PWM m2enA value 0-100
+volatile int PWMval3 = 0;   // PWM m3enA value 0-100
+
+
 Adafruit_MCP23X17 mcp2;
 
-void setup()
-{
- if (!mcp2.begin_I2C(0x22)) {
-  Serial.println("Error initializing MCP23017 at 0x22.");
-  while (1);
- }
- // set all the motor control pins to outputs
- mcp2.pinMode(m1enA, OUTPUT);
- mcp2.pinMode(m1enB, OUTPUT);
- mcp2.pinMode(m2enA, OUTPUT);
- mcp2.pinMode(m2enB, OUTPUT);
- mcp2.pinMode(m3enA, OUTPUT);
- mcp2.pinMode(m3enB, OUTPUT);
+void setup(){
+  if (!mcp2.begin_I2C(0x22)) {
+    Serial.println("Error initializing MCP23017 at 0x22.");
+    //while (1);
+  }
+  // set all the motor control pins
+  mcp2.pinMode(m1enA, OUTPUT);
+  mcp2.pinMode(m1enB, OUTPUT);
+  mcp2.pinMode(m2enA, OUTPUT);
+  mcp2.pinMode(m2enB, OUTPUT);
+  mcp2.pinMode(m3enA, OUTPUT);
+  mcp2.pinMode(m3enB, OUTPUT);
+  mcp2.pinMode(m1pwm1, OUTPUT);
+  mcp2.pinMode(m1pwm2, OUTPUT);
+  mcp2.pinMode(m2pwm1, OUTPUT);
+  mcp2.pinMode(m2pwm2, OUTPUT);
+  mcp2.pinMode(m3pwm1, OUTPUT);
+  mcp2.pinMode(m3pwm2, OUTPUT);
 
- mcp2.pinMode(m1pwm1, OUTPUT);
- mcp2.pinMode(m1pwm2, OUTPUT);
- mcp2.pinMode(m2pwm1, OUTPUT);
- mcp2.pinMode(m2pwm2, OUTPUT);
- mcp2.pinMode(m3pwm1, OUTPUT);
- mcp2.pinMode(m3pwm2, OUTPUT);
+  //set encoder pins
+  mcp2.pinMode(m1encA, INPUT);
+  mcp2.pinMode(m1encB, INPUT);
+  mcp2.pinMode(m2encA, INPUT);
+  mcp2.pinMode(m2encB, INPUT);
+  pinMode(m3encA, INPUT);
+  pinMode(m3encB, INPUT);
+  attachInterrupt(digitalPinToInterrupt(m1encA), AencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m1encB), AencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m2encA), BencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m2encB), BencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m3encA), CencoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(m3encB), CencoderISR, CHANGE);
 
- //set encoder pins to input
- mcp2.pinMode(m1encA, INPUT);
- mcp2.pinMode(m1encB, INPUT);
- mcp2.pinMode(m2encA, INPUT);
- mcp2.pinMode(m2encB, INPUT);
- pinMode(m3encA, INPUT);
- pinMode(m3encB, INPUT);
- attachInterrupt(digitalPinToInterrupt(m1encA), AencoderISR, CHANGE);
- attachInterrupt(digitalPinToInterrupt(m1encB), AencoderISR, CHANGE);
- attachInterrupt(digitalPinToInterrupt(m2encA), BencoderISR, CHANGE);
- attachInterrupt(digitalPinToInterrupt(m2encB), BencoderISR, CHANGE);
- attachInterrupt(digitalPinToInterrupt(m3encA), CencoderISR, CHANGE);
- attachInterrupt(digitalPinToInterrupt(m3encB), CencoderISR, CHANGE);
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM1); 
+  Timer3.attachInterrupt(updatePWM2); 
+  Timer3.attachInterrupt(updatePWM3); 
+  
 }
-void softpwm(int pin, int value){
+void updatePWM1() {
+  static int counter1 = 0;
 
+  if (counter1 < PWMval1) {
+    mcp2.digitalWrite(m1enA, HIGH);
+  } else {
+    mcp2.digitalWrite(m1enA, LOW);
+  }
 
+  counter1++;
+  if (counter1 >= 100) {
+    counter1 = 0;
+  }
+}
+void updatePWM2() {
+  static int counter2 = 0;
+
+  if (counter2 < PWMval2) {
+    mcp2.digitalWrite(m2enA, HIGH);
+  } else {
+    mcp2.digitalWrite(m2enA, LOW);
+  }
+
+  counter2++;
+  if (counter2 >= 100) {
+    counter2 = 0;
+  }
+}
+void updatePWM3() {
+  static int counter3 = 0;
+
+  if (counter3 < PWMval3) {
+    mcp2.digitalWrite(m3enA, HIGH);
+  } else {
+    mcp2.digitalWrite(m3enA, LOW);
+  }
+
+  counter3++;
+  if (counter3 >= 100) {
+    counter3 = 0;
+  }
+}
+void mcpsoftpwm(int pin, int vall){
+  //M1enA = 3   = GPA3
+  //M2enA = 7   = GPA7
+  //M3enA = 11  = GPB3
+  Serial.print("softPWM pin: ");
+  Serial.print(pin);
+  Serial.print(", value: ");
+  Serial.println(vall);
+  if (pin = 3){
+    PWMval1 = vall;
+    Serial.print("Motor 1 enA PWM set");
+  }
+  if (pin = 7){
+    PWMval2 = vall;
+    Serial.print("Motor 2 enA PWM set");
+  }
+  if (pin = 11){
+    PWMval3 = vall;
+    Serial.print("Motor 3 enA PWM set");
+  }
+  
 }
 void AencoderISR() {
   Aencount++;
@@ -144,47 +218,47 @@ void Creadenc() {
 }
 
 void Amove(double sped) {
-  spdA = abs(sped)*255;
-  mcp2.digitalWrite(m1enA, HIGH);
+  spdA = abs(sped) * 255;
+  mcpsoftpwm(m1enA, spdA);
   mcp2.digitalWrite(m1enB, LOW);
   if (sped < 0) {
     Serial.print("Motor A back ");
-    mcp2.analogWrite(m1pwm1, spdA);
-    mcp2.analogWrite(m1pwm2, 0);
+    mcp2.digitalWrite(m1pwm1, LOW);
+    mcp2.digitalWrite(m1pwm2, HIGH);
   } else {
     Serial.print("Motor A forward ");
-    mcp2.analogWrite(m1pwm1, 0);
-    mcp2.analogWrite(m1pwm2, spdA);
+    mcp2.digitalWrite(m1pwm1, HIGH);
+    mcp2.digitalWrite(m1pwm2, LOW);
   } 
   Serial.println(spdA);
 }
 void Bmove(double sped) {
-  spdB = abs(sped)*255;
-  mcp2.digitalWrite(m2enA, HIGH);
+  spdB = abs(sped) * 255;
+  mcpsoftpwm(m2enA, spdB);
   mcp2.digitalWrite(m2enB, LOW);
   if (sped < 0) {
     Serial.print("Motor B back ");
-    mcp2.analogWrite(m2pwm1, spdB);
-    mcp2.analogWrite(m2pwm2, 0);
+    mcp2.digitalWrite(m2pwm1, LOW);
+    mcp2.digitalWrite(m2pwm2, HIGH);
   } else {
     Serial.print("Motor B forward ");
-    mcp2.analogWrite(m2pwm1, 0);
-    mcp2.analogWrite(m2pwm2, spdB);
+    mcp2.digitalWrite(m2pwm1, HIGH);
+    mcp2.digitalWrite(m2pwm2, LOW);
   } 
   Serial.println(spdB);
 }
 void Cmove(double sped) {
   spdC = abs(sped) * 255;
-  mcp2.digitalWrite(m3enA, HIGH);
+  mcpsoftpwm(m3enA, spdC);
   mcp2.digitalWrite(m3enB, LOW);
   if (sped < 0) {
     Serial.print("Motor C back ");
-    mcp2.analogWrite(m3pwm1, spdC);
-    mcp2.analogWrite(m3pwm2, 0);
+    mcp2.digitalWrite(m3pwm1, LOW);
+    mcp2.digitalWrite(m3pwm2, HIGH);
   } else {
     Serial.print("Motor C forward ");
-    mcp2.analogWrite(m3pwm1, 0);
-    mcp2.analogWrite(m3pwm2, spdC);
+    mcp2.digitalWrite(m3pwm1, HIGH);
+    mcp2.digitalWrite(m3pwm2, LOW);
   } 
   Serial.println(spdC);
 }