Dear: LMR
Hello guys it is me Noah again just to let you know I have decided that the next project I am going to work on is a Rover 5. I managed to get the Motors moving I found that easy but not the encoders. I want to control the motors and rotary encoders in a simple Void called void Move(int RightMotorSpeed, int RightEncoderDegrees, int LeftMotorSpeed, int LeftEncoderDegrees ). For some reason I can get the robot moving but not the encoders. RightMotorSpeed is the speed for the right motor, RightEncoderDegrees is the amount of steps I want the right motor to make, LeftMotorSpeed is the speed of the left motor, and LeftEncoderDegrees is the amount of steps I want the left motor to make. Oh yeah I forgot to say I am using the dagu mini driver with the rover 5.
You should know my rover 5 is the two motored version with two rotary encoders and here is my wiring. My right motor is wired to the pins MR and my left motor is wired to pins ML. My rotary encoders is being supplied 5v and is connected to ground while my left rotary encoder has the yellow wire connected to d2 on the pwm pins and the white wire connected to d3 on the pwm pins. My other rotary encoder my right one and it has the yellow wire connected to d4 and my other wire connected to d5 on the pwm pins.
Can someone please help me I am kind of lost and thank you.
int const RightMotorDir = 7; int const LeftMotorDir = 8; int const RightMotorPWM = 10; int const LeftMotorPWM = 9;int const encoderLPA = 2;
int const encoderLPB = 3;
int const encoderRPA = 4;
int const encoderRPB = 5;int DegreesLeft = 1000;
int DegreesRight = 1000;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(RightMotorDir, OUTPUT);
pinMode(LeftMotorDir, OUTPUT);
pinMode(RightMotorPWM, OUTPUT);
pinMode(LeftMotorPWM, OUTPUT);pinMode(encoderLPA, INPUT);
pinMode(encoderLPB, INPUT);
pinMode(encoderRPA, INPUT);
pinMode(encoderRPB, INPUT);
}void loop() {
// put your main code here, to run repeatedly:
bool CanMove = true;
if (CanMove == true)
{
Move(255, DegreesRight, 255, DegreesLeft);
if (DegreesRight == 0 && DegreesLeft == 0)
{
CanMove = false;
}
}
}
void Move(int RightMotorSpeed, int RightEncoderDegrees, int LeftMotorSpeed, int LeftEncoderDegrees )
{
if (LeftEncoderDegrees > 0)
{
digitalWrite(LeftMotorDir, HIGH);
analogWrite(LeftMotorPWM, LeftMotorSpeed);
}
else
{
digitalWrite(LeftMotorDir, LOW);
analogWrite(LeftMotorPWM, LeftMotorSpeed);
}
if (RightEncoderDegrees > 0)
{
digitalWrite(RightMotorDir, HIGH);
analogWrite(RightMotorPWM, RightMotorSpeed);
}
else
{
digitalWrite(RightMotorDir, LOW);
digitalWrite(RightMotorPWM, RightMotorSpeed);
}
if (digitalRead(encoderLPA) == HIGH)
{
if (digitalRead(encoderLPB) == LOW)
{
LeftEncoderDegrees = LeftEncoderDegrees - 1;
}
else
{
LeftEncoderDegrees = LeftEncoderDegrees + 1;
}
}
else
{
if (digitalRead(encoderLPB) == HIGH)
{
LeftEncoderDegrees = LeftEncoderDegrees + 1;
}
else
{
LeftEncoderDegrees = LeftEncoderDegrees - 1;
}
}
if (digitalRead(encoderRPA) == HIGH)
{
if (digitalRead(encoderRPB) == LOW)
{
RightEncoderDegrees = RightEncoderDegrees - 1;
}
else
{
RightEncoderDegrees = RightEncoderDegrees + 1;
}
}
else
{
if (digitalRead(encoderRPB) == HIGH)
{
RightEncoderDegrees = RightEncoderDegrees + 1;
}
else
{
RightEncoderDegrees = RightEncoderDegrees - 1;
}
}
}
From: Noah
Dear: LMR
I finally figured out oddbot's code. I copied and pasted it into my dagu mini driver and I got it working. Sorry for the delay it took me a while to buy a new dagu mini driver since two of mine broke for no apparant reason but anyways. Enjoy my code!!!
From: Noah
PS: There is only one problem is that the robot is constanly leaning to the right. Anyways easy fix and thank you!!!
#include <Wire.h>#define L1encpin 0 // A0 is left encoder input 1
#define L2encpin 1 // A1 is left encoder input 2
#define R1encpin 2 // A2 is right encoder input 1
#define R2encpin 3 // A3 is right encoder input 2
#define lmdirpin 7 // D7 left motor direction
#define lmpwmpin 9 // D9 left motor PWM
#define rmdirpin 8 // D8 right motor direction
#define rmpwmpin 10 // D10 right motor PWM// D11 - D13 are servos 5-7
//========================================================== Encoder input variables ========================================================================
unsigned long lpulse; // width of left and right encoder pulses in uS
unsigned long rpulse; // width of left and right encoder pulses in uS
int lcount,rcount; // left and right pulse counters to measure distance
byte encold; // old state of encoders
byte encnew; // new state of encoders//========================================================== Motor control variables ========================================================================
int lpwm,rpwm; // left and right motor speeds generated from the processor
int lspeed,rspeed; // left and right motor speeds requested by the user
unsigned long maxspeed=520;
unsigned long actual; // stores temporary calculation of actual left and right motor speeds in uS between encoder pulsesunsigned long etime; // timer used to check encoders
unsigned long mtime; // timer used to check encoders
unsigned long ltime; // remembers time of left encoders last state change in uS
unsigned long rtime; // remembers time of right encoders last state change in uSvoid setup()
{
pinMode( lmdirpin, OUTPUT );
pinMode( lmpwmpin, OUTPUT );
pinMode( rmdirpin, OUTPUT );
pinMode( rmpwmpin, OUTPUT );Wire.begin(1); // join the I2C bus as slave device at address 1
Wire.onReceive(I2Ccommand); // go to I2Ccommand if I2C data received
}void loop()
{
lspeed = 100;
rspeed = 100;
//======================================================== PWM Generator ==================================================================================
if(micros()-etime>79) // timer generates 8 bit PWM at aproximately 50Hz
{
etime=micros(); // reset timer
static byte pwm;
pwm++; // byte “pwm” constanly increments (0-255)
digitalWrite(lmpwmpin,(pwm<lpwm/10));
digitalWrite(rmpwmpin,(pwm<rpwm/10));
}
//======================================================== Encoder Monitor ================================================================================encold=encnew; // previous new state now becomes old state
encnew=PINC&B00001111; // read new encoder stateif((encold&B00000011)!=(encnew&B00000011)) // compare old state of left encoders with new state of left encoders
{
lpulse=int((micros()-ltime)/50); // time between last state change and this state change
ltime=micros(); // update ltime with time of most recent state change
lcount++; // increment left motor distance counter
}
if((encold&B00001100)!=(encnew&B00001100)) // compare old state of right encoders with new state of right encoders
{
rpulse=int((micros()-rtime)/50); // time between last state change and this state change
rtime=micros(); // update ltime with time of most recent state change
rcount++; // increment right motor distance counter
}
//======================================================== Motor Speed Control ============================================================================
if(micros()-mtime>2999)
{
mtime=micros();
if(micros()-ltime>50000ul && lspeed!=0) lpwm+=4; // jumpstart stalled motor
if(micros()-rtime>50000ul && rspeed!=0) rpwm+=4; // jumpstart stalled motor
digitalWrite(lmdirpin,lspeed>0); // set direction of left motor
actual=20000/(abs(lspeed)maxspeed/255); // calculate desired time in uS between encoder pulses
if(actual>lpulse && lpwm>0) lpwm–; // if motor is running too fast then decrease PWM
if(actual<lpulse && lpwm<2549) lpwm++; // if motor is running too slow then increase PWM
//analogWrite(lmpwmpin,lpwm/10);
digitalWrite(rmdirpin,rspeed>0); // set direction of right motor
actual=20000/(abs(rspeed)maxspeed/255); // calculate desired time in uS between encoder pulses
if(actual>rpulse && rpwm>0) rpwm–; // if motor is running too fast then decrease PWM
if(actual<rpulse && rpwm<2549) rpwm++; // if motor is running too slow then increase PWM
//analogWrite(rmpwmpin,rpwm/10);
}
}
void I2Ccommand(int bytes)
{
if(bytes!=20 || Wire.available()!=20) // each data packet should consist of 10 integers (20 bytes)
{
EmptyBuffer();
return;
}
lspeed=Wire.read()256+Wire.read(); // read left motor speed integer
rspeed=Wire.read()256+Wire.read(); // read right motor speed integer
}//========================================================== Empty Buffer of Corrupt Data ===================================================================
void EmptyBuffer()
{
while(Wire.available()>0) // continue until buffer is empty
{
byte b=Wire.read(); // read byte from I2C buffer
}
}