summaryrefslogtreecommitdiff
path: root/CODE/CODE.ino
diff options
context:
space:
mode:
Diffstat (limited to 'CODE/CODE.ino')
-rw-r--r--CODE/CODE.ino373
1 files changed, 0 insertions, 373 deletions
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 <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 04:50:40 +0000