1 files changed, 89 insertions, 6 deletions
diff --git a/CODE/CODE.ino b/CODE/CODE.ino
index d923b2c..6b07d67 100644
--- a/CODE/CODE.ino
+++ b/CODE/CODE.ino
@@ -1,3 +1,12 @@
+#include <TimerThree.h>
+#include <Arduino.h>
+
+int speed = 1000; // speed 0-1
+
+volatile int PWMval1 = 0;   // PWM value 0-100
+volatile int PWMval2 = 0;   // PWM value 0-100
+volatile int PWMval3 = 0;   // PWM value 0-100
+
 int incomingbyte1 = 0;  //the byte that it is reciefing
 bool iswriting1 = false;  //true if writing else fals
 String tlocdat1 = "";  //temporary location data
@@ -57,7 +66,6 @@ int dpr6 = "LOW";
 int dpr7 = "LOW";
 int dpr8 = "LOW";
 
-
 void setup() {
   pinMode(m1enA, OUTPUT);
   pinMode(m1enB, OUTPUT);
@@ -88,6 +96,81 @@ void setup() {
   Serial.begin(9600);
   Serial1.begin(9600);
   Serial2.begin(9600);
+
+  Timer3.initialize(800); // 800 microseconds / 1.25kHz
+  Timer3.attachInterrupt(updatePWM1);
+  Timer3.attachInterrupt(updatePWM2);
+  Timer3.attachInterrupt(updatePWM3);
+}
+
+void updatePWM1() {
+  static int counter = 0;
+
+  if (counter < PWMval1) {
+    digitalWrite(m1enA, HIGH);
+  } else {
+    digitalWrite(m1enA, LOW);
+  }
+
+  counter++;
+  if (counter >= 100) {
+    counter = 0;
+  }
+}
+
+void updatePWM2() {
+  static int counter = 0;
+
+  if (counter < PWMval1) {
+    digitalWrite(m2enA, HIGH);
+  } else {
+    digitalWrite(m2enA, LOW);
+  }
+
+  counter++;
+  if (counter >= 100) {
+    counter = 0;
+  }
+}
+
+void updatePWM3() {
+  static int counter = 0;
+
+  if (counter < PWMval1) {
+    digitalWrite(m3enA, HIGH);
+  } else {
+    digitalWrite(m3enA, LOW);
+  }
+
+  counter++;
+  if (counter >= 100) {
+    counter = 0;
+  }
+}
+
+void mcpsoftpwm1(int value1) {
+  if (value1 < 0) value1 = 0;
+  if (value1 > 100) value1 = 100;
+
+  Serial.print("softPWM pin: 3, 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); 
+  PWMval2 = value2; 
+}
+void mcpsoftpwm3(int value3) {
+  if (value3 < 0) value3 = 0;
+  if (value3 > 100) value3 = 100;
+
+  Serial.print("softPWM pin: 3, value: ");
+  Serial.println(value3); 
+  PWMval3 = value3; 
 }
 
 void drive(int speed, int direction){
@@ -101,9 +184,9 @@ void drive(int speed, int direction){
  // 300 deg..............|....-....|....0....|....+....
  //
 
-  digitalWrite(m1enA, HIGH);
-  digitalWrite(m2enA, HIGH);
-  digitalWrite(m3enA, HIGH);
+  mcpsoftpwm1(speed);  //pwm speed signal
+  mcpsoftpwm2(speed);  //pwm speed signal
+  mcpsoftpwm2(speed);  //pwm speed signal
   digitalWrite(m1enB, LOW);
   digitalWrite(m2enB, LOW);
   digitalWrite(m3enB, LOW);
@@ -276,11 +359,11 @@ void loop() {
       }
       Serial.print(degwhere);
       Serial.println(" degrees.");
-      drive(255, degwhere);
+      drive(75, degwhere);
       delay(1);
     }
   }else{
-    Serial.println("softPWR is off, waiting")
+    Serial.println("softPWR is off, waiting");
     delay(100);  
   }
 }