soft PWM added

6 files changed, 441 insertions, 81 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);
 }
diff --git a/debug/motordriver/nomultiplexer/nomultiplexer.ino b/debug/motordriver/nomultiplexer/nomultiplexer.ino
index abfe14c..f6f839c 100644
--- a/debug/motordriver/nomultiplexer/nomultiplexer.ino
+++ b/debug/motordriver/nomultiplexer/nomultiplexer.ino
@@ -133,51 +133,52 @@ void Creadenc() {
 }
 
 void Amove(double sped) {
-  spdA = abs(sped)*255;
-  digitalWrite(m1enA, HIGH);
+  spdA = abs(sped) * 255.000;
+  analogWrite(m1enA, spdA);
   digitalWrite(m1enB, LOW);
   if (sped < 0) {
     Serial.print("Motor A back ");
-    analogWrite(m1pwm1, spdA);
-    analogWrite(m1pwm2, 0);
+    digitalWrite(m1pwm1, LOW);
+    digitalWrite(m1pwm2, HIGH);
   } else {
     Serial.print("Motor A forward ");
-    analogWrite(m1pwm1, 0);
-    analogWrite(m1pwm2, spdA);
+    digitalWrite(m1pwm1, HIGH);
+    digitalWrite(m1pwm2, LOW);
   } 
   Serial.println(spdA);
 }
 void Bmove(double sped) {
-  spdB = abs(sped)*255;
-  digitalWrite(m2enA, HIGH);
+  spdB = abs(sped) * 255.000;
+  analogWrite(m2enA, spdB);
   digitalWrite(m2enB, LOW);
   if (sped < 0) {
     Serial.print("Motor B back ");
-    analogWrite(m2pwm1, spdB);
-    analogWrite(m2pwm2, 0);
+    digitalWrite(m2pwm1, LOW);
+    digitalWrite(m2pwm2, HIGH);
   } else {
     Serial.print("Motor B forward ");
-    analogWrite(m2pwm1, 0);
-    analogWrite(m2pwm2, spdB);
+    digitalWrite(m2pwm1, HIGH);
+    digitalWrite(m2pwm2, LOW);
   } 
   Serial.println(spdB);
 }
 void Cmove(double sped) {
-  spdC = abs(sped) * 255;
-  digitalWrite(m3enA, HIGH);
+  spdC = abs(sped) * 255.000;
+  analogWrite(m3enA, spdC);
   digitalWrite(m3enB, LOW);
   if (sped < 0) {
     Serial.print("Motor C back ");
-    analogWrite(m3pwm1, spdC);
-    analogWrite(m3pwm2, 0);
+    digitalWrite(m3pwm1, LOW);
+    digitalWrite(m3pwm2, HIGH);
   } else {
     Serial.print("Motor C forward ");
-    analogWrite(m3pwm1, 0);
-    analogWrite(m3pwm2, spdC);
+    digitalWrite(m3pwm1, HIGH);
+    digitalWrite(m3pwm2, LOW);
   } 
   Serial.println(spdC);
 }
 
+
 void moveohmi(int speed, int direction){
   Amove(0);
   Bmove(0);
diff --git a/debug/motordriver/onemotor/onemotor.ino b/debug/motordriver/onemotor/onemotor.ino
index f3d91e6..33f0c5c 100644
--- a/debug/motordriver/onemotor/onemotor.ino
+++ b/debug/motordriver/onemotor/onemotor.ino
@@ -1,5 +1,3 @@
-#include <Arduino.h>
-
 int enA = 1;
 int enB = 2;
 int pwm1 = 3;
@@ -7,7 +5,7 @@ int pwm2 = 4;
 int encoA = 5;
 int encoB = 6;
 
-int speed = 100; // speed 0-100
+int speed = 1; // speed 0-1
 
 double sp = 0.00;
 double rpm = 0.00;
@@ -56,17 +54,17 @@ void resetenc(){
 }
 
 void move(int sped) {
-  sp = abs(sped)*2.55;
-  digitalWrite(enA, HIGH);
+  sp = abs(sped)*255;
+  analogWrite(enA, sp);
   digitalWrite(enB, LOW);
   if (sped < 0) {
     Serial.print("back ");
-    analogWrite(pwm1, sp);
-    analogWrite(pwm2, 0);
+    digitalWrite(pwm1, LOW);
+    digitalWrite(pwm2, HIGH);
   } else {
     Serial.print("forward ");
-    analogWrite(pwm1, 0);
-    analogWrite(pwm2, sp);
+    digitalWrite(pwm1, HIGH);
+    digitalWrite(pwm2, LOW);
   } 
   Serial.println(sp);
 }
diff --git a/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino b/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino
new file mode 100644
index 0000000..b9335f3
--- /dev/null
+++ b/debug/motordriver/onemotor_softPWM/onemotor_softPWM.ino
@@ -0,0 +1,117 @@
+#include <TimerThree.h>
+
+int enA = 1;  //PWM pin
+int enB = 2;  //GND
+int pwm1 = 3; //
+int pwm2 = 4;
+int encoA = 5;
+int encoB = 6;
+
+int speed = 1; // speed 0-1
+
+double sp = 0.00;
+double rpm = 0.00;
+volatile long encount = 0;
+unsigned long Ltime = 0;
+unsigned long Ctime = 0;
+unsigned long Ptime = 0;
+
+volatile int PWMval = 0;   // PWM value 0-100
+
+
+void setup() {
+  pinMode(enA, OUTPUT);
+  pinMode(enB, OUTPUT);
+  pinMode(pwm1, OUTPUT);
+  pinMode(pwm2, OUTPUT);
+
+  pinMode(encoA, INPUT_PULLUP);
+  pinMode(encoB, INPUT_PULLUP);
+
+  attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE);
+
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM); 
+
+  Serial.begin(9600);
+}
+
+void updatePWM() {
+  static int counter = 0;
+
+  if (counter < PWMval) {
+    digitalWrite(enA, HIGH);
+  } else {
+    digitalWrite(enA, LOW);
+  }
+
+  counter++;
+  if (counter >= 100) {
+    counter = 0;
+  }
+}
+void mcpsoftpwm(int value) {
+  if (value < 0) value = 0;
+  if (value > 100) value = 100;
+
+  Serial.print("softPWM pin: enA, value: ");
+  Serial.println(value); 
+  PWMval = value; 
+}
+
+void encoderISR() {
+  encount++;
+}
+
+void readenc() {
+  Ctime = micros();
+  Ptime = Ctime - Ltime;
+  Ltime = Ctime;
+  rpm = (encount / 211.2) * (60000000.0 / Ptime);
+  Serial.print("RPM: ");
+  Serial.print(rpm);
+  Serial.print("    Sample period:");
+  Serial.print(Ptime);
+  Serial.print("ms, encoder count:");
+  Serial.println(encount);
+
+  encount = 0;
+}
+void resetenc(){
+  Ctime = micros();
+  Ltime = Ctime;
+  encount = 0;
+}
+
+void move(int sped) {
+  sp = abs(sped)*100;
+  mcpsoftpwm(sp);
+  digitalWrite(enB, LOW);
+  if (sped < 0) {
+    Serial.print("back ");
+    digitalWrite(pwm1, LOW);
+    digitalWrite(pwm2, HIGH);
+  } else {
+    Serial.print("forward ");
+    digitalWrite(pwm1, HIGH);
+    digitalWrite(pwm2, LOW);
+  } 
+  Serial.println(sp);
+}
+
+void loop() {
+  move(speed);
+  delay(1000);
+  readenc();
+
+  
+  move(0);
+  resetenc();
+  delay(1000);
+  readenc();
+
+  move(-speed);
+  delay(1000);
+  readenc();
+}
diff --git a/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino b/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino
new file mode 100644
index 0000000..37128ce
--- /dev/null
+++ b/debug/motordriver/onemotor_softPWM_multiplexer/onemotor_softPWM_multiplexer.ino
@@ -0,0 +1,124 @@
+#include <TimerThree.h>
+#include <Adafruit_MCP23X17.h>
+
+Adafruit_MCP23X17 mcp;
+
+int enA = 0;    // GPA0 -PWM
+int enB = 1;    // GPA1 -GND
+int pwm1 = 2;   // GPA2 -HIGH/LOW
+int pwm2 = 3;   // GPA3 -HIGH/LOW
+int encoA = 4;  // GPA4 -read
+int encoB = 5;  // GPA5 -read
+
+int speed = 1; // speed 0-1
+
+double sp = 0.00;
+double rpm = 0.00;
+volatile long encount = 0;
+unsigned long Ltime = 0;
+unsigned long Ctime = 0;
+unsigned long Ptime = 0;
+
+volatile int PWMval = 0;   // PWM value 0-100
+
+
+void setup() {
+  Serial.begin(9600);
+  if (!mcp.begin_I2C(0x20)) {
+    Serial.println("Error initializing MCP23017 at 0x20.");
+    while (1);
+  }
+
+  mcp.pinMode(enA, OUTPUT);
+  mcp.pinMode(enB, OUTPUT);
+  mcp.pinMode(pwm1, OUTPUT);
+  mcp.pinMode(pwm2, OUTPUT);
+
+  mcp.pinMode(encoA, INPUT_PULLUP);
+  mcp.pinMode(encoB, INPUT_PULLUP);
+
+  attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE);
+
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM); 
+}
+
+void updatePWM() {
+  static int counter = 0;
+
+  if (counter < PWMval) {
+    mcp.digitalWrite(enA, HIGH);
+  } else {
+    mcp.digitalWrite(enA, LOW);
+  }
+
+  counter++;
+  if (counter >= 100) {
+    counter = 0;
+  }
+}
+void mcpsoftpwm(int value) {
+  if (value < 0) value = 0;
+  if (value > 100) value = 100;
+
+  Serial.print("softPWM pin: enA, value: ");
+  Serial.println(value); 
+  PWMval = value; 
+}
+
+void encoderISR() {
+  encount++;
+}
+
+void readenc() {
+  Ctime = micros();
+  Ptime = Ctime - Ltime;
+  Ltime = Ctime;
+  rpm = (encount / 211.2) * (60000000.0 / Ptime);
+  Serial.print("RPM: ");
+  Serial.print(rpm);
+  Serial.print("    Sample period:");
+  Serial.print(Ptime);
+  Serial.print("ms, encoder count:");
+  Serial.println(encount);
+
+  encount = 0;
+}
+void resetenc(){
+  Ctime = micros();
+  Ltime = Ctime;
+  encount = 0;
+}
+
+void move(int sped) {
+  sp = abs(sped)*100;
+  mcpsoftpwm(sp);
+  mcp.digitalWrite(enB, LOW);
+  if (sped < 0) {
+    Serial.print("back ");
+    mcp.digitalWrite(pwm1, LOW);
+    mcp.digitalWrite(pwm2, HIGH);
+  } else {
+    Serial.print("forward ");
+    mcp.digitalWrite(pwm1, HIGH);
+    mcp.digitalWrite(pwm2, LOW);
+  } 
+  Serial.println(sp);
+}
+
+void loop() {
+  move(speed);
+  delay(1000);
+  readenc();
+
+  
+  move(0);
+  resetenc();
+  delay(1000);
+  readenc();
+
+  move(-speed);
+  delay(1000);
+  readenc();
+}
diff --git a/debug/motordriver/softPWM/softPWM.ino b/debug/motordriver/softPWM/softPWM.ino
new file mode 100644
index 0000000..ab74715
--- /dev/null
+++ b/debug/motordriver/softPWM/softPWM.ino
@@ -0,0 +1,46 @@
+#include <TimerThree.h>
+#include <Arduino.h>
+
+int pwmPin = 3;
+
+volatile int PWMval = 0;   // PWM value 0-100
+
+void setup() {
+  pinMode(pwmPin, OUTPUT);
+  Serial.begin(9600);
+
+  Timer3.initialize(800); // 800 microseconds or 1.25kHz
+  Timer3.attachInterrupt(updatePWM); 
+}
+
+void updatePWM() {
+  static int counter = 0;
+
+  if (counter < PWMval) {
+    digitalWrite(pwmPin, HIGH);
+  } else {
+    digitalWrite(pwmPin, LOW);
+  }
+
+  counter++;
+  if (counter >= 100) {
+    counter = 0;
+  }
+}
+
+void mcpsoftpwm(int value) {
+  if (value < 0) value = 0;
+  if (value > 100) value = 100;
+
+  Serial.print("softPWM pin: 3, value: ");
+  Serial.println(value); 
+  PWMval = value; 
+}
+
+void loop() {
+  mcpsoftpwm(100);  // Set PWM to 100%
+  delay(1000);
+  mcpsoftpwm(50);   // Set PWM to 50%
+  delay(1000);
+  Serial.println("loop");
+}