updated repo structure

1 files changed, 373 insertions, 0 deletions

diff --git a/OLD_CODE_REMOVE_LATER/code/teensy_code/teensy_code.ino b/OLD_CODE_REMOVE_LATER/code/teensy_code/teensy_code.ino
new file mode 100644
index 0000000..f6918a7
--- /dev/null
+++ b/OLD_CODE_REMOVE_LATER/code/teensy_code/teensy_code.ino
@@ -0,0 +1,373 @@
+#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
+String locdat1 = "";   //location data
+int nir1 = "-99"; // which ir sensor it is
+int pir1 = "-99"; // amount of pulses it had
+
+int incomingbyte2 = 0;  //same but for seeed xiao N 2
+bool iswriting2 = false;
+String tlocdat2 = ""; 
+String locdat2 = ""; 
+int nir2 = "-99";
+int pir2 = "-99";
+
+float degwhere = -99.0; // where the ball is in degrees
+
+int led = 13;
+
+int m1enA = 6;
+int m1enB = 5;
+int m1pwm1 = 4;
+int m1pwm2 = 3;
+int m1encoA = 32;
+int m1encoB = 31;
+
+int m2enA = 12;
+int m2enB = 11;
+int m2pwm1 = 9;
+int m2pwm2 = 10;
+int m2encoA = 30;
+int m2encoB = 29;
+
+int m3enA = 27;
+int m3enB = 26;
+int m3pwm1 = 24;
+int m3pwm2 = 25;
+int m3encoA = 28;
+int m3encoB = 23;
+
+int softpwr = 2;
+
+int pr1 = 13;
+int pr2 = 41;
+int pr3 = 40;
+int pr4 = 39;
+int pr5 = 38;
+int pr6 = 37;
+int pr7 = 36;
+int pr8 = 35;
+
+int dpr1 = "LOW";
+int dpr2 = "LOW";
+int dpr3 = "LOW";
+int dpr4 = "LOW";
+int dpr5 = "LOW";
+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);
+  pinMode(m1pwm1, OUTPUT);
+  pinMode(m1pwm2, OUTPUT);
+  
+  pinMode(m2enA, OUTPUT);
+  pinMode(m2enB, OUTPUT);
+  pinMode(m2pwm1, OUTPUT);
+  pinMode(m2pwm2, OUTPUT);
+  
+  pinMode(m3enA, OUTPUT);
+  pinMode(m3enB, OUTPUT);
+  pinMode(m3pwm1, OUTPUT);
+  pinMode(m3pwm2, OUTPUT);
+
+  pinMode(softpwr, INPUT_PULLUP);
+
+  pinMode(pr1, INPUT);
+  pinMode(pr2, INPUT);
+  pinMode(pr3, INPUT);
+  pinMode(pr4, INPUT);
+  pinMode(pr5, INPUT);
+  pinMode(pr6, INPUT);
+  pinMode(pr7, INPUT);
+  pinMode(pr8, INPUT);
+
+  Serial.begin(9600);
+  Serial3.begin(9600);
+  Serial2.begin(9600);
+
+  Timer3.initialize(800); // 800 microseconds / 1.25kHz
+  Timer3.attachInterrupt(updatePWM); 
+}
+
+void updatePWM() {
+  static int counter = 0;
+
+  if (counter < PWMval1) {
+    digitalWrite(m1enA, HIGH);
+  } else {
+    digitalWrite(m1enA, LOW);
+  }
+  
+  if (counter < PWMval2) {
+    digitalWrite(m2enA, HIGH);
+  } else {
+    digitalWrite(m2enA, LOW);
+  }
+  
+  if (counter < PWMval3) {
+    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: 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: enA, value: ");
+  Serial.println(value2); 
+  PWMval2 = value2; 
+}void mcpsoftpwm3(int value3) {
+  if (value3 < 0) value3 = 0;
+  if (value3 > 100) value3 = 100;
+
+  Serial.print("softPWM pin: enA, value: ");
+  Serial.println(value3); 
+  PWMval3 = value3; 
+}
+
+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....|....-....|....+....
+ // 60deg................|....+....|....-....|....0....    
+ // 120deg...............|....+....|....0....|....-....
+ // 180 deg (backward)...|....0....|....+....|....-.... 
+ // 240 deg..............|....-....|....+....|....0....
+ // 300 deg..............|....-....|....0....|....+....
+ //
+ if(direction>330 || direction<=30){
+    //0 deg
+    //Serial.println("drive 0 deg"); 
+    move1(0);
+    move2(-speeeed);
+    move3(speeeed);
+  }else if (direction>30 && direction <= 90){
+    //60 deg
+    //Serial.println("drive 60deg");
+    move1(speeeed);
+    move2(-speeeed);
+    move3(0);
+  }else if (direction>90 && direction <= 150){
+    //120 deg
+    //Serial.println("drive 120 deg");
+    move1(speeeed);
+    move2(0);
+    move3(-speeeed);
+  }else if (direction > 150 && direction <= 210) {
+    //180 degrees
+    //Serial.println("drive  180 deg");
+    move1(0);
+    move2(speeeed);
+    move3(-speeeed);
+  }else if (direction > 210 && direction <= 270) {
+    //240 degrees
+    //Serial.println("drive 240 deg");
+    move1(-speeeed);
+    move2(speeeed);
+    move3(0);
+  }else if (direction > 270 && direction <= 330) {
+    //300 degrees
+    //Serial.println("drive 300 deg");
+    move1(-speeeed);
+    move2(0);
+    move3(speeeed);
+  }else{
+    Serial.println("how?");  
+  }
+}
+
+void loop() {
+  int sprst = digitalRead(softpwr);  //softpower status
+  if(sprst == LOW){
+    dpr1 = digitalRead(pr1);
+    dpr2 = digitalRead(pr2); 
+    dpr3 = digitalRead(pr3); 
+    dpr4 = digitalRead(pr4); 
+    dpr5 = digitalRead(pr5); 
+    dpr6 = digitalRead(pr6); 
+    dpr7 = digitalRead(pr7); 
+    dpr8 = digitalRead(pr8); 
+  
+    if(dpr1 == HIGH){
+      //go back to ?deg
+    }else if (dpr2 == HIGH){
+      //go back to ?deg
+    }else if (dpr3 == HIGH){
+      //go back to ?deg
+    }else if (dpr4 == HIGH){
+      //go back to ?deg
+    }else if (dpr5 == HIGH){
+      //go back to ?deg
+    }else if (dpr6 == HIGH){
+      //go back to ?deg
+    }else if (dpr7 == HIGH){
+      //go back to ?deg
+    }else if (dpr8 == HIGH){
+      //go back to ?deg
+    }else{
+      if (Serial3.available() > 0) {
+        incomingbyte1 = Serial3.read();
+        //Serial.print(char(incomingbyte1));
+        if (incomingbyte1 == 's'){
+          iswriting1 = true;
+          tlocdat1 = "";
+        }else if(incomingbyte1 == 'e'){
+          iswriting1 = false;
+          locdat1 = tlocdat1;        
+        }else if(iswriting1 == true){
+          //Serial.print(char(incomingbyte1));
+          tlocdat1 += (char)incomingbyte1;  
+        }
+      }
+      if (Serial2.available() > 0) {
+        incomingbyte2 = Serial2.read();
+        //Serial.print(char(incomingbyte2));
+        if (incomingbyte2 == 's'){
+          iswriting2 = true;
+          tlocdat2 = "";
+        }else if(incomingbyte1 == 'e'){
+          iswriting2 = false;
+          locdat2 = tlocdat1;        
+        }else if(iswriting2 == true){
+          //Serial.print(char(incomingbyte1));
+          tlocdat2 += (char)incomingbyte1;  
+        }
+      }
+      //Serial.println(locdat);
+      sscanf(locdat1.c_str(), "%d, %d", &nir1, &pir1);
+      sscanf(locdat2.c_str(), "%d, %d", &nir2, &pir2);
+
+      Serial.print("locdat: ");
+      Serial.print(String(locdat1));
+      Serial.println(String(locdat2));
+      
+      
+      if(pir1 > pir2){
+         if(nir1 = 1){    //not good looking code because posbile wrong wireing
+          degwhere = 0;      
+         }else if(nir1 = 2){
+          degwhere = 22.5;
+         }else if(nir1 = 3){
+          degwhere = 45;
+         }else if(nir1 = 4){
+          degwhere = 67.5;
+         }else if(nir1 = 5){
+          degwhere = 90;
+         }else if(nir1 = 6){
+          degwhere = 112.5;
+         }else if(nir1 = 7){
+          degwhere = 135;
+         }else if(nir1 = 8){
+          degwhere = 157.5;
+         }
+      }else{     
+         if(nir2 = 1){
+          degwhere = 180;
+         }else if(nir2 = 2){
+          degwhere = 202.5;
+         }else if(nir2 = 3){
+          degwhere = 225;
+         }else if(nir2 = 4){
+          degwhere = 247.5;
+         }else if(nir2 = 5){
+          degwhere = 270;
+         }else if(nir2 = 6){
+          degwhere = 292.5;
+         }else if(nir2 = 7){
+          degwhere = 315; 
+         }else if(nir2 = 8){
+          degwhere = 337.5; 
+         }
+      }
+
+      Serial.print("Ball is at ");
+      Serial.print(degwhere);
+      Serial.println(" degrees.");
+      drive(75, degwhere);
+      delay(1);
+    }
+  }else{
+    Serial.println("softPWR is off, waiting");
+    delay(100);  
+  }
+}