Ok, well, Ive been fiddling with this on my days off. And so far I got both encoders working fine. Thou I don't think either my code, Or the ardunio isn't fast enough to capture the encoder ticks at very high speeds.
I am completely aware that this code is very well sloped together, But for the moment it works. I still need to do my distance calculations based on wheel circumference. I also need to make the code more friendly. But yea the whole project needs more work, its currently tied down with masking tape, and magnet wire. Ive had problems like the wheels sucking in wires and unplugging things like encoders.
Code for Motor speed, and setup
#include "WProgram.h"
#define byte uint8_t
class Motor{
public:
int L1,L2,SpeedPin,Speed;
void MotorSet(int SetL1,int SetL2,int SetSpeedPin,int SetSpeed){
L1=SetL1;
L2=SetL2;
SpeedPin=SetSpeedPin;
Speed=SetSpeed;
pinMode(L1, OUTPUT);
pinMode(L2, OUTPUT);
pinMode(SpeedPin, OUTPUT);
}
int GetSpeed(){
return(Speed);
}
int SetSpeed(int SetSP){
Speed=SetSP;
}
int Forward(){
digitalWrite(L1,LOW);
digitalWrite(L2,HIGH);
analogWrite(SpeedPin,Speed);
}
int Reverse(){
digitalWrite(L1,HIGH);
digitalWrite(L2,LOW);
analogWrite(SpeedPin,Speed);
}
int Stop(){
digitalWrite(L1,LOW);
digitalWrite(L2,LOW);
digitalWrite(SpeedPin,LOW);
}
};
Code for encoders and whatnot, Now I had a bit of a issue with this before it wasn't using any arrays and the Input pin0,pin1 were messing around with each other and the code just basically went berserk. This fixed it for now with the least amount of space.
class Step{
public:
int ReadVal[2];
int ResetReadValue(){
ReadVal[0]=0;
ReadVal[1]=0;
}
int Rstop(int P){
if(P==0)ReadVal[0]=1;
if(P==1)ReadVal[1]=1;
}
int Steps(int Rot,int Pin){
static int ReadEnc[2]={0,0},OldEncRead[2]={0,0},clicks[2]={0,0};
ReadEnc[Pin] = analogRead(Pin);
ReadEnc[Pin]=map(ReadEnc[Pin], 0, 1023, 0, 2); //Originally I had the encoders on Digital inputs but I switched it to analog with no issues using map(); and now I have 2 more free Digital pins.
if(ReadEnc[Pin] != OldEncRead[Pin]) {
if(clicks[Pin] == Rot) {
clicks[Pin] = 0;
Serial.println(Pin);
Rstop(Pin);
return(0);
}
else clicks[Pin]++;
OldEncRead[Pin] = ReadEnc[Pin];
return(0);
}
return(0);
}
};
Actual program code. Needs alot of work. The whole action variable was to keep it from redoing the code it just did.
#include <Motor.h>
#include <Step.h>
Step Enc1,Enc2;
Motor M1,M2;
void setup() {
M1.MotorSet(10,11,6,120);
M2.MotorSet(9,8,5,120);
Enc1.ResetReadValue();
Enc2.ResetReadValue();
Serial.begin(115200);
}
int Action=0;
int S1(int Tick1,int Tick2,char Dir1,char Dir2,int Speed1,int Speed2){
while(Action==0){
M1.SetSpeed(Speed1);
M2.SetSpeed(Speed2);
Enc1.Steps(Tick1,1);
Enc2.Steps(Tick2,0);
if((Enc1.ReadVal[1]!=1) && (Dir1=='F'))M1.Forward();
if((Enc1.ReadVal[1]!=1) && (Dir1=='R'))M1.Reverse();
if(Enc1.ReadVal[1]==1)M1.Stop();
if((Enc2.ReadVal[0]!=1) && (Dir2=='F'))M2.Forward();
if((Enc2.ReadVal[0]!=1) && (Dir2=='R'))M2.Reverse();
if(Enc2.ReadVal[0]==1)M2.Stop();
if((Enc1.ReadVal[1]==1)&&(Enc2.ReadVal[0]==1)&&(Action==0)){
Action=1;
Enc1.ResetReadValue();
Enc2.ResetReadValue();
Action=0;
return(0);
}
}
}
int e,f;
char a,b;
void loop()
{
for(int i=60;i<255;i++){
e=random(0,2);
f=random(0,2);
if(e==0)a='F';
if(e==1)a='R';
if(f==0)b='F';
if(f==1)b='R';
S1(random(20,60),random(20,60),a,b,i,i);
}
}
![]( "- YouTube")
Short video of both wheels turning.Sorry for crap quality I'm not used to recording with a 35fps garbage Microsoft web cam (I think my cellphone has better fps). If one finishes its rotation before the other it waits until its done. Then both start off, Near the end of the video it bugs out because it got to the end of the for loop and a variable got stuck. Also it was going so fast I though I was gonna strip some gears. lol
And to answer any questions about whats at the front with the servo type wire. Its a ping sensor got it for about 34$ CAD, Works awesome. Its currently not coded.
