I am working with a Nexus 3WD Robot with 6 sonar sensors. I’ve been trying to make sense of the sample 3WD code that was included in the manual, concerning functions like setSpeedMMPS, etc. At first I tried to get the robot to move straight by simplifying the included sample code. I came up with this:
[code]#include <Wire.h>
#include <Urm5parser.h>
#include <SRF02.h>
#include <IR.h>
#include <SONAR.h>
#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>
#include <fuzzy_table.h>
#include <PID_Beta6.h>
#include <MotorWheel.h>
#include <Omni3WD.h>
#include <Omni4WD.h>
#include <Omni4WDAction.h>
#include <R2WD.h>
#include <EEPROM.h>
irqISR(irq1,isr1); // Intterrupt function.on the basis of the pulse ,work for wheel1
MotorWheel wheel1(9,8,6,7,&irq1); //This will create a MotorWheel object called Wheel1
//Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7
irqISR(irq2,isr2);
MotorWheel wheel2(10,11,12,13,&irq2);
irqISR(irq3,isr3);
MotorWheel wheel3(3,2,4,5,&irq3);
//MotorWheel wheel3(5,4,2,3,&irq3);
// why not this?? Because the pin 5,pin 6 control by timer 0
Omni3WD Omni(&wheel1,&wheel2,&wheel3);
// This will create a Omni3WD object called Omni3WD.
//You can then use any of its methods; for instance,
void setup() {
TCCR1B=TCCR1B&0xf8|0x01; // Timer1.Pin9,Pin10 PWM 31250Hz
TCCR2B=TCCR2B&0xf8|0x01; // Timer2 .Pin3,Pin11 PWM 31250Hz
Omni.PIDEnable(0.26,0.02,0,10); // Enable PID
}
void loop() {
unsigned int speedMMPS = 100;
unsigned int duration = 1000;
unsigned int uptime = 1000;
boolean debug = false;
Omni.setCarAdvance(speedMMPS);
Omni.setCarSpeedMMPS(speedMMPS,uptime);
}[/code]
The setCarAdvance function sets the speed of each wheel individually - the rear wheel speed is 0 and the left and right wheel speeds are equal, but in opposite directions (speedMMPS). I would think that this code would make the robot go straight by turning the left and right wheels at the same speed (in this case, 100 mm/s). However, this code causes one wheel to spin at a much faster speed than the other - making the robot turn rather than go straight. I have tried different values for speedMMPS with similar results.
Since I could not get this working, I decided to try to work with the more basic functions like wheelRightSetSpeed, etc. to see if I could set the speed of each wheel independently. These are the functions that are inside the setCarAdvance function. I tried code like this:
[code]void loop() {
unsigned int speedMMPS = 100;
unsigned int duration = 1000;
unsigned int uptime = 1000;
boolean debug = false;
Omni.wheelBackSetSpeedMMPS(0,DIR_ADVANCE);
Omni.wheelRightSetSpeedMMPS(speedMMPS,DIR_BACKOFF);
Omni.wheelLeftSetSpeedMMPS(speedMMPS,DIR_ADVANCE);
}[/code]
But nothing happens. I figured that if I could accurately set the speeds of individual wheels, I could just write my own functions for things like go straight, turn left, etc., but for some reason setting the speed of individual wheels does not work.
I would appreciate any advice on this, because I’ve working on it for awhile to no avail.
Thank you,
John