From 764c8722596f5b3bd01bb26848f7289b0972d7b0 Mon Sep 17 00:00:00 2001 From: Abel Tim Date: Sun, 23 Mar 2025 12:38:48 +0100 Subject: updated repo structure --- CODE/CODE.ino | 373 ---------------------------------------------------------- 1 file changed, 373 deletions(-) delete mode 100644 CODE/CODE.ino (limited to 'CODE/CODE.ino') diff --git a/CODE/CODE.ino b/CODE/CODE.ino deleted file mode 100644 index f6918a7..0000000 --- a/CODE/CODE.ino +++ /dev/null @@ -1,373 +0,0 @@ -#include -#include - -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); - } -} -- cgit v1.2.3