Drive

colorMovimiento24.ino

@@ -0,0 +1,58 @@
+/*
+#include "Color.h"
+#include "BNO.h"
+#include "Motores.h" 
+
+Color colorSensor;
+BNO gyro;
+Motores motores(5, 28, 27, 6, 25, 26, 4, 30, 29);
+
+void setup() {
+  Serial.begin(9600);
+  colorSensor.iniciar();
+  colorSensor.calibrar();
+  gyro.iniciar();
+  motores.iniciar();
+}
+
+void loop() {
+  double anguloLinea = colorSensor.checkForLinea();
+
+  if (anguloLinea >= 0) {
+    int anguloLineaNuevo = (anguloLinea + 180);
+    int anguloLineaN = (anguloLineaNuevo%360); 
+    motores.movimientoLineal(anguloLineaN, 50);
+    Serial.print("angulo linea: ");  
+    Serial.print(anguloLinea);  
+    Serial.print("angulo a donde se mueve");  
+    Serial.println(anguloLineaN);  
+    delay(100); 
+  } else {
+    motores.apagarMotores();
+    }
+} 
+
+*/
+#include "Color.h"
+
+Color colorSensor; 
+
+void setup() {
+  Serial.begin(9600);
+  colorSensor.iniciar();  
+  colorSensor.calibrar(); 
+}
+
+void loop() {
+  double anguloLinea = colorSensor.checkForLinea();
+
+  if (anguloLinea >= 0) {
+    Serial.print("Angulo de la linea detectada: ");
+    Serial.println(anguloLinea);
+  } else {
+    Serial.println("No se detecta la línea blanca.");
+  }
+
+  delay(1000);   
+
+} 

drive/BNO.cpp

@@ -0,0 +1,71 @@
+#include "Arduino.h"
+#include "BNO.h"
+
+BNO::BNO() {
+}
+
+void BNO::iniciar() {
+  if (!bno.begin(OPERATION_MODE_IMUPLUS)) {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    while (1);
+  }
+  bno.setExtCrystalUse(true);
+}
+
+void BNO::readValues() {
+
+  sensors_event_t orientationData;
+  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
+  yaw = (double)orientationData.orientation.x;
+  // convert yaw angle to range -180 to 180
+  yaw = (yaw > 0) ? (360 - yaw) : yaw;
+  yaw = (yaw > 180) ? (yaw - 360) : yaw;
+
+  if (offset > 0) {
+    yaw = (yaw > 0) ? (-180 + yaw) : (180 + yaw);
+  }
+  // determine the direction 
+  right = (yaw >= 0) ? false : true;
+}
+
+
+void BNO::displayCalStatus()
+{
+  /* Get the four calibration values (0..3) */
+  /* Any sensor data reporting 0 should be ignored, */
+  /* 3 means 'fully calibrated" */
+  uint8_t system, gyro, accel, mag;
+  system = gyro = accel = mag = 0;
+  bno.getCalibration(&system, &gyro, &accel, &mag);
+
+  /* The data should be ignored until the system calibration is > 0 */
+   Serial.print("\t");
+
+  /* Display the individual values */
+  Serial.print("Sys:");
+  Serial.print(system, DEC);
+  Serial.print(" G:");
+  Serial.print(gyro, DEC);
+  Serial.print(" A:");
+  Serial.print(accel, DEC);
+  Serial.print(" M:");
+  Serial.println(mag, DEC);
+}
+
+
+double BNO::getYaw() {
+  return yaw;
+}
+
+double BNO::getMag() {
+  return mag;
+}
+
+void BNO::setOffset(double off) {
+  offset = off;
+}
+
+bool BNO::isRight() {
+  return right;
+}

drive/BNO.h

@@ -0,0 +1,28 @@
+#ifndef BNO_h
+  #define BNO_h
+
+  #include <Wire.h>
+  #include <Adafruit_Sensor.h>
+  #include <Adafruit_BNO055.h>
+
+  class BNO {
+    private:
+      double yaw;
+      bool right = true;
+      Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire);
+      double offset = 0;
+      double mag = 0;
+
+    public:
+
+      BNO();
+      void iniciar();
+      void readValues();
+      double getYaw();
+      double getMag();
+      void setOffset(double off);
+      bool isRight();
+      void displayCalStatus();
+  };
+
+#endif

drive/drive.cpp

@@ -0,0 +1,111 @@
+#include "Arduino.h"
+#include "drive.h"
+
+drive::drive (int m1Speed, int m1P1, int m1P2, int m2Speed, int m2P1, int m2P2, int m3Speed, int m3P1, int m3P2) {
+  //parameters for each motor
+  motor1.set(m1Speed, m1P1, m1P2);
+  motor2.set(m2Speed, m2P1, m2P2);
+  motor3.set(m3Speed, m3P1, m3P2);
+}
+
+void drive::initialize() {
+  motor1.startMotor();
+  motor2.startMotor();
+  motor3.startMotor();
+}
+
+//kinematic equations for robot movement
+void drive::linealMovement(int degree, int speed) {
+
+  float m1 = sin(((60 - degree) * PI / 180));
+  float m2 = sin(((180 - degree) * PI / 180));;
+  float m3 = sin(((300 - degree) * PI / 180));
+  int speedA = abs(int(m1 * speed));
+  int speedB = abs(int(m2 * speed));
+  int speedC = abs(int(m3 * speed));
+
+  //set motor speeds
+  analogWrite(motor1.getMotorSpeed(), speedA);
+  analogWrite(motor2.getMotorSpeed(), speedB);
+  analogWrite(motor3.getMotorSpeed(), speedC);
+
+  //rotate motors based on the angles
+  if (m1 >= 0) {
+    motor1.motorFrontward();
+  } else {
+    motor1.motorBackward();
+  }
+
+  if (m2 >= 0) {
+    motor2.motorFrontward();
+  } else {
+    motor2.motorBackward();
+  }
+
+  if (m3 >= 0) {
+    motor3.motorFrontward();
+  } else {
+    motor3.motorBackward();
+  }
+
+}
+
+//set speed for motors
+void drive::setAllMotorSpeed(int allSpeed) {
+  analogWrite(motor1.getMotorSpeed(), allSpeed);
+  analogWrite(motor2.getMotorSpeed(), allSpeed);
+  analogWrite(motor3.getMotorSpeed(), allSpeed);
+}
+
+//turn off motors
+void drive::driveOff() {
+  motor1.stopMotor();
+  motor2.stopMotor();
+  motor3.stopMotor();
+}
+
+void drive::driveFrontward() {
+  driveOff();
+  motor1.motorBackward();
+  motor2.motorFrontward();
+}
+
+void drive::driveBackward() {
+  motor1.motorFrontward();
+  motor2.motorBackward();
+}
+
+//tests for various angles
+void drive::movementTester(int speeds) {
+  linealMovement(0, speeds);
+  delay(1000);
+
+  linealMovement(60, speeds);
+  delay(1000);
+
+  linealMovement(180, speeds);
+  delay(1000);
+
+  linealMovement(300, speeds);
+  delay(1000);
+
+  linealMovement(45, speeds);
+  delay(1000);
+
+  linealMovement(90, speeds);
+  delay(1000);
+
+  linealMovement(135, speeds);
+  delay(1000);
+}
+
+//functions for moving each motor 
+void drive::move1() {
+  motor1.motorFrontward();
+}
+void drive::move2() {
+  motor2.motorFrontward();
+}
+void drive::move3() {
+  motor3.motorFrontward();
+}

drive/drive.h

@@ -0,0 +1,28 @@
+#ifndef drive_h
+  #define drive_h
+
+  #pragma once
+  #include "motor.h"
+  #include "Arduino.h"
+
+  class drive {
+    public:
+      motor motor1;
+      motor motor2;
+      motor motor3;
+
+      drive (int m1Speed, int m1P1, int m1P2, int m2Speed, int m2P1, int m2P2, int m3Speed, int m3P1, int m3P2);
+      void initialize();
+      void linealMovement(int degree, int velocidad);
+      void setAllMotorSpeed(int allSpeed);
+      void driveOff();
+      void driveFrontward();
+      void driveBackward();
+      void movementTester(int velocidades);
+      void move1();
+      void move2();
+      void move3();
+
+  };
+
+#endif

drive/drive.ino

@@ -0,0 +1,54 @@
+#include "Arduino.h"
+#include "drive.h"
+
+// pin configuration
+const int m1Speed = 4; 
+const int m1P1 = 23;
+const int m1P2 = 22;
+const int m2Speed = 5;
+const int m2P1 = 24;
+const int m2P2 = 25;
+const int m3Speed = 6;
+const int m3P1 = 26;
+const int m3P2 = 27;
+
+// speed for testing and angle
+const int testSpeed = 100;
+
+void setup() {
+  Serial.begin(9600);
+
+  // initialize drive system
+  drive robotDrive(m1Speed, m1P1, m1P2, m2Speed, m2P1, m2P2, m3Speed, m3P1, m3P2);
+  robotDrive.initialize();
+}
+
+void loop() {
+  //linealMovement for the specified angle
+  drive robotDrive(m1Speed, m1P1, m1P2, m2Speed, m2P1, m2P2, m3Speed, m3P1, m3P2);
+  robotDrive.linealMovement(60, testSpeed);
+    delay(1000);
+  robotDrive.linealMovement(180, testSpeed);
+    delay(1000);
+  robotDrive.linealMovement(300, testSpeed);
+    delay(1000);
+  robotDrive.driveOff(); 
+  delay(1000);
+
+}
+
+/*pins for motors: 
+
+M1: 4
+in1= 22
+in2 = 23
+
+M2: 5
+in1= 25
+in2 = 24
+
+M3: 6
+in1= 27
+in2 = 26
+
+*/