Notifications
Clear all
Topic starter
2024-11-01 8:25 am
Hi all.
Just posting some code for comment or suggestions.
There are two buttons. While btn1 is held down motorA will run and while btn2 is held down motorB will run. Holding down both buttons results in both motors running.
If both buttons are held down then the robot would move in a straight line if the wheels were turning at the same rate. A drift to the left will decrease the angle theta and a drift to the right will increase the angle theta. The angle is the direction the robot is moving and the x,y is the current position of the robot relative to some absolute frame of reference.
Can this data be used to keep a robot on course by auto adjusting the PWM values?
Regardless of how fast each of the wheels turn the encoder values under ideal conditions will enable the software to compute the current position of the robot and direction it is travelling.
Below is a test program which uses the Arduino IDE Serial Monitor to print the current position and direction of the robot which will change if one or both motors are running.
The units of distance measure are actually in encoder ticks for which there would be some constant for any give motor to translate into common units like centimeters.
I run the program with the robot base lying on its back where I can watch the motors and I have to reach in to press the buttons on the bread board. The robot doesn't "know" it isn't actually moving about only where it would be if it was moving about.
// 564 ticks per wheel rotation
// 7cm diameter wheels
// 29cm axel width
// 22cm circumference of wheels
// 26 ticks per cm
// button pins assignments
const int btn1 = 24; // press button
const int btn2 = 26;
// Motor A
int enA = 11;
int in1 = 10;
int in2 = 9;
int counter1 = 0;
// Motor B
int enB = 5;
int in3 = 7;
int in4 = 6;
int PWM1 = 240;
int PWM2 = 240;
int counter2 = 0;
// **** ENCODER ODOMETRY VARIABLES ****
float DL = 0; // amount left wheel has rotated
float DR = 0; // amount right wheel has rotated
float x,y; // position in world
float theta; // direction
float temp; // for radian to degree conversion
float expr1;
float w = 754; // 29 cm axel (w value in ticks not cm)
// ============================================== //
int prevCounter1 = 0;
int prevCounter2 = 0;
int flagB = 0; // flag motorB off
int flagA = 0; // flag motorA off
void turnOnMotorA(int rate){
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
analogWrite(enA, rate);
}
void turnOnMotorB(int rate){
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enB, rate);
}
void turnOffMotorA(){
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
}
void turnOffMotorB(){
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
void setup()
{
// buttons pinMode
pinMode(btn1,INPUT_PULLUP);
pinMode(btn2,INPUT_PULLUP);
// Set all the motor control pins to outputs
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(3,INPUT); // set as input
digitalWrite(3,HIGH); // enable internal pullup resister
attachInterrupt(digitalPinToInterrupt(3), encoder2, RISING); // interrupt initialization
pinMode(2,INPUT); // set as input
digitalWrite(2,HIGH); // enable internal pullup resister
attachInterrupt(digitalPinToInterrupt(2), encoder1, RISING); // interrupt initialization
analogWrite(enA, 0);
analogWrite(enB, 0);
turnOffMotorA();
turnOffMotorB();
counter1 = 0;
counter2 = 0;
prevCounter1 = 0;
prevCounter2 = 0;
// set x,y to origin
x = 0.0;
y = 0.0;
Serial.begin( 9600 );
Serial.println("Starting up");
delay(1000);
}
// interrrupt server routines for reading encoder
void encoder1()
{
counter1 = counter1 + 1;
}
void encoder2()
{
counter2 = counter2 + 1;
}
void loop(){
if (digitalRead(btn1) == 0){
turnOnMotorA(PWM1);
}else{
turnOffMotorA();
}
if (digitalRead(btn2) == 0){
turnOnMotorB(PWM2);
}else{
turnOffMotorB();
}
unsigned long startTime = millis();
// move along a bit before seeing how much each wheel has rotated
while ( (millis() - startTime) < 50){}
DL = (float)counter1 - (float)prevCounter1;
DR = (float)counter2 - (float)prevCounter2;
if (DL==DR) {
// Moving in a straight line
x = x + DL * cos(theta);
y = y + DR * sin(theta);
}else{
// Moving in an arc
expr1 = w * (DR + DL)/ 2.0 / (DR - DL);
x = x + expr1 * (sin( (DR - DL) / w + theta) - sin(theta));
y = y - expr1 * (cos( (DR - DL) / w + theta) - cos(theta));
// Calculate new orientation
theta = theta + ((float)DR - (float)DL) / (float)w;
// Keep in the range -PI to +PI
if (theta > PI) {
theta = theta - (2.0*PI);
}
if (theta < -PI) {
theta = theta + (2.0*PI);
}
}
prevCounter1 = counter1;
prevCounter2 = counter2;
// print variable values
if (digitalRead(btn1) == 0 || digitalRead(btn2) == 0 ){
Serial.print("x =");
Serial.print(x);
Serial.print(" y =");
Serial.print(y);
Serial.print(" theta =");
temp = theta * (180/PI);
if (temp < 0){
temp = 360+temp;
}
Serial.println(temp); //print in degrees
Serial.println();
Serial.print("DL = ");
Serial.print(DL);
Serial.print(" DR = ");
Serial.println(DR);
}
}
Topic starter
2024-11-01 12:54 pm
@robotbuilder - your post is blank.
And yet if you look at the list on the right you can see the first part of the post????
@thrandell @byron Hi all.Just posting some code for comme...
So I don't know what is happening. It has happened before.
Perhaps Bill knows what the problem is?
Topic starter
2024-11-01 1:05 pm
It has also now vanished from the listing as well?
Trying clicking a "Quote" reply on the blank post. When I do that I can see the post in the quote part of a reply.
2024-11-01 3:19 pm
For what it’s worth all the early code that I posted on the forum, following instructions that you used yourself, resulted a large mess. Probably after an upgrade was applied to the forum software, IDK. Anyway Bill cleared it up by removing the offensive code from all my posts. I noticed that It affected your posts as well. Now I use the instructions found here: https://forum.dronebotworkshop.com/add-code/ I have to say that it's a pain cutting and pasting from my Macbook because it double spaces the pasted code and I have to manually remove those blank lines, but at least it doesn't disappear. Good Luck.
Tom
To err is human.
To really foul up, use a computer.
Topic starter
2024-11-01 5:31 pm
Still not working. Now a red window pops up saying the post needs more than 2 characters but it has more than 2 characters!!
In the past I would click the <> and paste the code into the resulting page that came up and it seemed to work fine. Pasting it directly into the post as suggested in the link you gave removes the indents and then when I highlighted the code in the post and clicked the code button <> as suggested it doesn't restore the indents as claimed.
2024-11-01 6:31 pm
@robotbuilder Yeah, I'm losing the indentation too. At least it's not double spaced...
void updateLocation(void) {
printf("In update Location\n");
float dRight, dLeft, dCenter;
float newX, newY, phi, newTheta;
//int16_t leftCnt, rightCnt;
dLeft = distPerClick * getCountsAndResetLeft();
dRight = distPerClick * getCountsAndResetRight();
// print the current pose
//leftCnt = getCountsAndResetLeft();
//rightCnt = getCountsAndResetRight();
//dLeft = distPerClick * leftCnt;
//dRight = distPerClick * rightCnt;
//printf("Current pose, x = %d y = %d and theta = %f\n", x, y, theta);
//printf("dLeft = %f dRight = %f leftCnt = %d rightCnt = %d\n", dLeft, dRight, leftCnt, rightCnt);
// Calculation to find new pose (x,y,th)
dCenter = (dLeft + dRight) / 2;
phi = (dRight - dLeft) / baseline;
newTheta = theta + phi;
newX = x + (dCenter * cos(theta));
newY = y + (dCenter * sin(theta));
if (fabs(newTheta) > twoPiRadians)
theta = (fabs(newTheta) - twoPiRadians) * signfloat(newTheta);
else
theta = newTheta;
x = trunc(newX); y = trunc(newY);
dCenterRunningTotal += dCenter;
// print the new pose.
printf("New pose, x = %d y = %d and theta = %f dCenterRunningTotal = %f\n", x, y, theta, dCenterRunningTotal);
}
To err is human.
To really foul up, use a computer.
Topic starter
2024-11-01 7:14 pm
Indents in Python are a requirement.
If it isn't fixed I could start each line with a special character and then when it is copied all you have to do is use find/replace to erase that character.
2024-11-01 10:28 pm
I also had something funny on post I made a week or so back when I tried to post a reply to a post to someone, forget who and what the subject was, but after typing out my post and hitting submit something briefly popped up on the screen, but my attention was elsewhere at the time and I only caught this in the corner of my eye. But the end result was the post window disappeared, and so did my post! I did not make a repost as I was called away. So sometimes we seem to get a few glitches with the forum site.
Maybe you will have more luck if you simply upload a file that others can download instead of using those <>. Its probably more easily readable as code can then be readily popped into ones favourite code editor and get colour highlighting and such delights.
2024-11-02 2:51 pm
@thrandell @robotbuilder I have not had trouble but my procedure is diferent
1. Select all in IDE
2. Copy in IDE
3. In Forum click the <>
4. Paste
First computer 1959. Retired from my own computer company 2004.
Hardware - Expert in 1401, 360, fairly knowledge in PC plus numerous MPU's & MCU's
Major Languages - Machine language, 360 Macro Assembler, Intel Assembler, PL/I and PL1, Pascal, Basic, C plus numerous job control and scripting languages.
My personal scorecard is now 1 PC hardware fix (circa 1982), 1 open source fix (at age 82), and 2 zero day bugs in a major OS.
Topic starter
2024-11-02 4:33 pm
That is the procedure I have always followed. When I click the <> icon the code window appears. I paste into the code window and click [OK].
The code appears in the post as expected. When I then [ADD REPLY] the post shows up in the listing on the right of the dronebot forum webpage but on clicking it the post is blank even though the first few words are visible in the webpage listing. I can choose to [EDIT] the blank post and everything is there but fails to appear after an edit. Then it all vanishes.
Anyway I tested and found you can indeed Attach an .ino file so that is what I will do in future.
It appears I can still attach images without any problems.
2024-11-02 7:09 pm
@robotbuilder FYI @thrandell Let's test, I will post the blink sketch below, let's see what happens.
/*
Blink
Turns an LED on for one second, then off for one second, repeatedly.
Most Arduinos have an on-board LED you can control. On the UNO, MEGA and ZERO
it is attached to digital pin 13, on MKR1000 on pin 6. LED_BUILTIN is set to
the correct LED pin independent of which board is used.
If you want to know what pin the on-board LED is connected to on your Arduino
model, check the Technical Specs of your board at:
https://www.arduino.cc/en/Main/Products
modified 8 May 2014
by Scott Fitzgerald
modified 2 Sep 2016
by Arturo Guadalupi
modified 8 Sep 2016
by Colby Newman
This example code is in the public domain.
https://www.arduino.cc/en/Tutorial/BuiltInExamples/Blink
*/
// the setup function runs once when you press reset or power the board
void setup() {
// initialize digital pin LED_BUILTIN as an output.
pinMode(LED_BUILTIN, OUTPUT);
}
// the loop function runs over and over again forever
void loop() {
digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
delay(1000); // wait for a second
digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
delay(1000); // wait for a second
}
Preview is correct
First computer 1959. Retired from my own computer company 2004.
Hardware - Expert in 1401, 360, fairly knowledge in PC plus numerous MPU's & MCU's
Major Languages - Machine language, 360 Macro Assembler, Intel Assembler, PL/I and PL1, Pascal, Basic, C plus numerous job control and scripting languages.
My personal scorecard is now 1 PC hardware fix (circa 1982), 1 open source fix (at age 82), and 2 zero day bugs in a major OS.
2024-11-02 7:10 pm
Looks ok so far
First computer 1959. Retired from my own computer company 2004.
Hardware - Expert in 1401, 360, fairly knowledge in PC plus numerous MPU's & MCU's
Major Languages - Machine language, 360 Macro Assembler, Intel Assembler, PL/I and PL1, Pascal, Basic, C plus numerous job control and scripting languages.
My personal scorecard is now 1 PC hardware fix (circa 1982), 1 open source fix (at age 82), and 2 zero day bugs in a major OS.