summaryrefslogtreecommitdiff
path: root/OLD_CODE_REMOVE_LATER/code/teensy_code/teensy_code.ino
diff options
context:
space:
mode:
Diffstat (limited to 'OLD_CODE_REMOVE_LATER/code/teensy_code/teensy_code.ino')
-rw-r--r--OLD_CODE_REMOVE_LATER/code/teensy_code/teensy_code.ino373
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);
+ }
+}
generated by cgit v1.2.3 (git 2.46.0) at 2026-09-30 00:43:42 +0000