diff --git a/SLAM_ROBOT/Arduino/Motorz.h b/SLAM_ROBOT/Arduino/Motorz.h new file mode 100644 index 0000000..085c7de --- /dev/null +++ b/SLAM_ROBOT/Arduino/Motorz.h @@ -0,0 +1,91 @@ +#ifndef _MOTORZ_H // not #ifnotdef +#define _MOTORZ_H + +class Motorz +{ + private: + int motor_speed=0; + int enable,in1,in2,dir; + public: + + Motorz(int Enb,int ina,int inb){ + enable=Enb; + in1=ina; + in2=inb; + pinMode(enable,OUTPUT); + pinMode(in1,OUTPUT); + pinMode(in2,OUTPUT); + } + Motorz(int Enb,int ina,int inb,int d){ + enable=Enb; + in1=ina; + in2=inb; + dir=d; + pinMode(enable,OUTPUT); + pinMode(in1,OUTPUT); + pinMode(in2,OUTPUT); + Direction(); + } + void SetSpeed(int Mspeed){ + motor_speed=Mspeed; + } + int GetSpeed(){ + return motor_speed; + } + void SetEnable(int pin){ + enable=pin; + } + int GetEnable(){ + return enable; + } + void SetInput1(int pin){ + in1=pin; + } + int GetInput1(){ + return in1; + } + void SetInput2(int pin){ + in2=pin; + } + int GetInput2(){ + return in2; + } + void SetDirection(int a){ + dir=a; + } + int GetDirection(){ + return dir; + } + void ChangeDir(){ + if(dir==0){ + dir=1; + } + else{ + dir=0; + } + } + void Start(){ + this->Direction(); + analogWrite(enable,motor_speed); + } + void Stop(){ + this->Halt(); + } + private: + void Direction(){ + if(dir==0){ + digitalWrite(in1,HIGH); + digitalWrite(in2,LOW); + } + else{ + digitalWrite(in1,LOW); + digitalWrite(in2,HIGH); + } + } + void Halt(){ + digitalWrite(in1,LOW); + digitalWrite(in2,LOW); + } + +}; +#endif _MOTOR_H diff --git a/SLAM_ROBOT/Arduino/arduino.ino b/SLAM_ROBOT/Arduino/arduino.ino new file mode 100644 index 0000000..880de5b --- /dev/null +++ b/SLAM_ROBOT/Arduino/arduino.ino @@ -0,0 +1,76 @@ +#include "Motorz.h"; +#include "tankdrive.h"; + +int i; +int enr=5; // enable pin for motor r (must be pwm) +int in1=4; // pin for one of the two connections of the mottor +int in2=3;// other connection (these two are digital pins) +// depends on how you end up wiring and placing the motors it will spin a certain way +// and another way when you reverse the voltage , direct holds (0,1) depending on how you +// wired the motor to go "forwards" what ever that means for your project +int directr=0; +int enl=6; +int in3=7; +int in4=8; +int directl=0; + +//buffer reseaving messages from esp32 +char buffer1[(sizeof(char)+sizeof(unsigned long))*sizeof(char)+1]; +char a; //character to read L/R/F character from esp32 +unsigned long runforsmth; // how long to run the motors. +Motorz motorR(enr,in1,in2,directr);//Right motor for the tankdrive system +Motorz motorL(enl,in3,in4,directl);//Left motor for the tankdrive system + +//object to manage the movement of the robot +TankDrive mytank(&motorR,directr,&motorL,directl); + +void setup(){ + Serial.begin(9600); //serial to esp32 make sure baud rates match + motorR.SetSpeed(80); //Set speed of right motor + motorL.SetSpeed(80); //Set speed of left motor + +} + + +void loop(){ + //read message from arduino and run the motors + //for the time and direction dictated + + while (Serial.available()) { + Serial.readBytes(buffer1,sizeof(buffer1)); + sscanf( buffer1, "%c%lu",&a, &runforsmth); + run_for_millis(&mytank,a,runforsmth); + } +} + +void run_for_millis(TankDrive *tank,char ch,unsigned long timetorun){ + + unsigned long count; + switch(ch){ + case 'L': //going to rotate left for timetorun millis + count=millis(); + do{ + tank->TurnLeft();} + while(millis()-count<=timetorun); + tank->Stop(); + break; + + case 'F'://going to move forward for timetorun millis + count=millis(); + do{ + tank->MoveForward();} + while(millis()-count<=timetorun); + tank->Stop(); + break; + case 'R'://going to rotate right for timetorun millis + count=millis(); + do{ + tank->TurnRight();} + while(millis()-count<=timetorun); + tank->Stop(); + break; + } + + + +} diff --git a/SLAM_ROBOT/Arduino/tankdrive.h b/SLAM_ROBOT/Arduino/tankdrive.h new file mode 100644 index 0000000..45485b3 --- /dev/null +++ b/SLAM_ROBOT/Arduino/tankdrive.h @@ -0,0 +1,66 @@ +#ifndef _TANKDRIVE_H // not #ifnotdef +#define _TANKDRIVE_H + +#include "Motorz.h" + +class TankDrive +{ + Motorz *motorL; + Motorz *motorR; + int forwardR,forwardL,backR,backL; + public: + TankDrive(Motorz *MR,int fr,Motorz *ML,int fl){ + motorL=ML; + motorR=MR; + forwardR=fr; + forwardL=fl; + if(fr==1)backR=0; + else backR=1; + if(fl==1)backL=0; + else backL=1; + } + void MoveForward(){ + motorR->SetDirection(forwardR); + motorL->SetDirection(forwardL); + motorR->Start(); + motorL->Start(); + } + void MoveBack(){ + motorR->SetDirection(backR); + motorL->SetDirection(backL); + motorR->Start(); + motorL->Start(); + } + void Stop(){ + + motorR->Stop(); + motorL->Stop(); + } + void TurnRight(){ + motorR->SetDirection(backR); + motorL->SetDirection(forwardL); + motorR->Start(); + motorL->Start(); + } + void TurnLeft(){ + motorR->SetDirection(forwardR); + motorL->SetDirection(backL); + motorL->Start(); + motorR->Start(); + } + int getFDirR(){ + return forwardR; + } + int getFDirL(){ + return forwardL; + } + int getBDirR(){ + return backR; + } + int getBDirL(){ + return backL; + } +}; + + +#endif _TANKDRIVE_H