Robot Base - new build

@byron 

... hopefully it will be back to the robot stuff sometime in the next year. 😀

 Have a nice break. I am still on a roll although today I dropped the encoder stuff and started again  experimenting and trying some new ideas I had using visual navigation on the robot base.

@byron
Just in case you get back to this subject using your AI accelerated camera to detect visual targets.

FYI @thrandell

The problem is:
Given the coordinates of TWO visual targets (beacons) and their compass directions how do you determine the coordinates of the robot?

This computes the intersections of two lines. One line is drawn through x1,y1 at angle a1 degrees and the other line is drawn through x2,y2 at angle a2 degrees. The lines will intersect at x5,y5.

This is a C++ example where you can change the input values x1,y1,x2,y2,a1,a2.

I don't have a c++ compiler so I used an online compiler.

https://www.programiz.com/cpp-programming/online-compiler/

You could use Arduino IDE and run the code on an Arduino using the Serial Monitor to enter values and output the results.

https://www.circuitbasics.com/how-to-read-user-input-from-the-arduino-serial-monitor/

I don't actually have Python installed so when I do I can post a Python version as Python is the most popular language and can be used with AI to detect a visual target.

// Online C++ compiler to run C++ program online
#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>

int main() {


    float x1,y1,x2,y2,x3,y3,x4,y4,x5,y5;
    float a1,a2;

    // sample positions of two beacons
    x1 = 150;
    y1 = 150;
    x2 = 500;
    y2 = 300;
    // compass direction of visual beacons
    a1 = 63;
    a2 = 164;


    x3 = x1+cos(a1*(M_PI/180));
    y3 = y1+sin(a1*(M_PI/180));

    x2 = x2;
    x4 = x2+cos(a2*(M_PI/180));
    y2 = y2;
    y4 = y2+sin(a2*(M_PI/180));
    
    float aa1 = y3 - y1;
    float b1 = x1 - x3;
    float c1 = aa1 * x1 + b1 * y1;
    float aa2 = y4 - y2;
    float b2 = x2 - x4;
    float c2 = aa2 * x2 + b2 * y2;
    
    float det = aa1 * b2 - aa2 * b1;

    if (det != 0) {
        x5 = (b2 * c1 - b1 * c2) / det;
        y5 = (aa1 * c2 - aa2 * c1) / det;
    }else{
        std::cout << "ERROR" << std::endl;
    }
    

    std::cout << " x5 =" << x5 << "  y5 =" << y5;


    return 0;
}

@robotbuilder  your message just popped up as I was reading my last emails (mainly about final enticements for buying good Friday deals 😎) before I retire for the night.   

Thanks for the example which I will look at in more detail over the next few days.  

I did something like this with OpenCV in Python a while back, though my example code I'm thinking of was from the standpoint of an overhead camera that saw the position of the bot on an imaginary grid (the camera view) with an imaginary north (the top centre of the view frame). The OpenCV gave the x,y coordinates of the current position of the bot (in the camera frame) and calculated a 'compass' bearing to an x,y coordinate on the grid as input as the desired point to reach.   I'll look it out in case its of any use.  Calculating the bearing from the standpoint of the camera being on the bot and with a real compass bearing of the bot is of course a different approach and one I'm sure I will find useful when I get back to bots with my bot based camera 👍  

@robotbuilder

From a quick look this morning for my old OpenCV bearing calc I did not find anything obvious and as I no longer have OpenCV loaded onto a rpi to check out the program when I find it I give up on this for now.   I will have to load up OpenCV onto my rpi where the AI camera is currently attached, but thats for the future.

I have been sorting out my files and have all my micropython stuff in good order, but the older rpi python files have not yet been trawled through and reorganised.  More fun to come.

But as I see you are possibly having a go at loading up python and getting some bearing calcs done I give a small GPS bearing / distance calc example that runs in both python and micropython.   The program will run on your new ESP32's if you choose to load up micropython on them.   As  I just got a new rpi pico 2 W and I was checking to to see if the new hardware floating point would let the floating point calculation be as accurate as when running python on the mac computer (it was 😀 and a good improvement over the first rpi pico), and whilst this program was to hand I thought I would pop it in this post for your amusement.

The program is a simple example of taking the latitude and longitude of a start and end point and then calculating both the distance between them and the compass bearing to get from start point to end point.  For the example I used google maps to get the lat an lon of HMS Belfast stern to bow thats moored in the Thames and also for London Bridge.   Maybe it helps remind you on how nice using python is 😎. 

I'm surprised your computer does not have a C++ complier already installed as this comes as standard on the mac.   My windows computer, an old gigantic metal cased thing, has been retired so I cant check on what comes with MS Windows.

Anyway I'm off to have another play with the pico2 W to check that some of my other programs run OK before shutting it all away for playtime sometime in the new year.

So below is the python snippet which when run gives the following output:

MPY: soft reboot
from t1_start to t1_stop
Bearing from start : 286.8 Degrees
Distance Metres (3 dec) : 186.859 Metres

from t2_start to t2_stop
Bearing from start : 12.5 Degrees
Distance Metres (3 dec) : 232.237 Metres

from math import asin, cos, radians, sin, sqrt, atan2, degrees

# for Python and Micropython
# Lat, Lon
# HMS Belfast stern to bow
t1_start = (51.506334, -0.080025)
t1_stop = (51.506819, -0.082610)
# London Bridge South to North (river bank to bank)
t2_start = (51.506950, -0.088087)
t2_stop = (51.508989, -0.087359)

def compass_bearing(p1, p2):
    # compass bearing from first lat,lon (point1) to second lat, lon (point2)
    lat1 = radians(p1[0])
    lat2 = radians(p2[0])
    diffLong = radians(p2[1] - p1[1])
    x = sin(diffLong) * cos(lat2)
    y = cos(lat1) * sin(lat2) - (sin(lat1)
            * cos(lat2) * cos(diffLong))
    initial_bearing = atan2(x, y)
    initial_bearing = degrees(initial_bearing)
    compass_bearing = round(((initial_bearing + 360) % 360) ,1)
    return compass_bearing

def gps_distance(p1, p2):
    # haversine calculation formula
    r = 6371  # Earth radius in kilometers
    lon_1, lon_2 = radians(p1[1]), radians(p2[1])
    lat_1, lat_2 = radians(p1[0]), radians(p2[0])
    h = (sin((lat_2 - lat_1) / 2)**2
            + cos(lat_1) * cos(lat_2) * sin((lon_2 - lon_1) / 2)**2)
    return 2 * r * asin(sqrt(h))
   
print('from t1_start to t1_stop')
print('  Bearing from start      : ', compass_bearing(t1_start, t1_stop), ' Degrees')
print('  Distance Metres (3 dec) : ', '{:.3f}'.format((gps_distance(t1_start, t1_stop)) * 1000),'Metres' )
print()
print('from t2_start to t2_stop')
print('  Bearing from start      : ', compass_bearing(t2_start, t2_stop), ' Degrees')
print('  Distance Metres (3 dec) : ', '{:.3f}'.format((gps_distance(t2_start, t2_stop)) * 1000),'Metres')
print()

Posted by: @robotbuilder

↑

The problem is:
Given the coordinates of TWO visual targets (beacons) and their compass directions how do you determine the coordinates of the robot?

Where did you find this formula?  Did you come up with it?

Tom

Posted by: @thrandell

↑

Where did you find this formula?  Did you come up with it?

Tom

https://rosettacode.org/wiki/Find_the_intersection_of_two_lines

@byron I have a Mac, what is the C++ compiler called, I have never heard of that.

@zander  

Though I've not tried it I think the xcode app has the ability to compile c and c++ as well as swift and uses the CLang compiler to do this.  I've only used the included c++ compiler from the command line.  If you do g++ --help from the command line what do you get?  

If you want a quick test than save the code below as first.cpp and the compile it with:

g++ -o RonTest first.cpp

and you should get an executable (on your mac) call RonTest.

(though before getting in to any c++ malarky you should try python 😀)

#include <iostream>
#include <string>
using namespace std;

// first c++

/* and this is writing a 
blooming book comment */

/* to compile - c++ % g++ test1.cpp -o runtest1
   to run - ./runtest1
*/

const int mynum = 28; //note constants must be initialised

int main() {
    int y,z;
    int x = 2;
    string hello = "Hello World "; // note ""
        for (int i = 0; i < hello.length(); i++) {
            cout << hello[i] << endl;
        }

    char myChar = 'B';  // note ''
    cout << "hello world " << x ;
    cout << "\n" << hello << x << "\n";
    y = 5;
    z = y + x;
    cout << "y = " << y << "\n" ;
    cout << "z = " << z;
    cout << " \n myChar = " << myChar << endl;
    cout << "myconst = " << mynum << endl;
    
    cout << "Give me a number for x: " << endl;
    cin >> x;
    cout << x << endl;
    cin.clear();
    cin.ignore(1000, '\n'); // ignor upto 1000 character of input if error!
    cout << "Give me a number for y: " << endl;
    cin >> y;
    cout << y << endl;
    z = y + x;
    cout << " x + y = " << z << endl;

    cout << "Give me a string: " << endl;
    cin >> hello;
    cout << "You entered: " << hello << endl;

    /* logical operators
        !   not
        &&  and
        ||  or
    */
    bool ax = false && true;
    bool ay = !true;
    bool az = true || false;
    bool azz = false || true;
    bool azzz = false || false;
    cout << "ax " << ax << endl;
    cout << "ay " << ay << endl;
    cout << "az " << az << endl;
    cout << "azz " << azz << endl;
    cout << "azzz " << azzz << endl;

    if (ax == true){
        cout << "ax is true" << endl;
    }

    if (ay == true){
        cout << "ay is true" << endl;
    } else{
        cout << "ay is false" << endl;
    }

    int mynum = 10;
    if (mynum > 10){
        cout << "mynum is GT 10" << endl;
    } else if (mynum >= 5){
        cout << "mynum is GT 4 but LT 11" << endl;
    } else {
        cout << "mynum is LT5" << endl;
    }
    
    int a = 8;
    int myArray[a];

    int myArray2[5] = {1,2,3,4,5};
    int myArray1[] = {1,2,3,4};
    int myArray3[] = {1,2,3};

    // int myArray4[];  NO GOOD
    // cout << myArray3[3];  NO GOOD only goes to myArray3[2] just give gibberish but no error!!

    for(int i = 0; i < 10; i++){
        cout << i << endl;
    }
    for (int i = 0; i < sizeof(myArray2)/sizeof(myArray2[0]); i++){
        cout << i << " ";
        cout << myArray2[i] << endl;
    }

    int input = -1;
    while (input != 1 && input !=2){
        cout << "Type a number 1 or 2: ";
        cin >> input;
    }

    input = -1;
    while (true){
        cout << "Type a number 1 or 2: ";
        cin >> input;
        if (input == 1 || input == 2){
            break;
        }
    }
    
    for (int i = 0; i < 10; i++){
        if (i == 9 || i == 7){
            continue;
        }
        cout << i << endl;
    }

    int myNum = 10;
    do{
        int add;
        cout << "Type a number to add to myNum: ";
        cin >> add;
        myNum += add;
        cout << "myNum is: " << myNum << endl;
    } while (myNum < 10);

    int whatever = 10;
    switch (whatever + 1){
        case 10:
            cout << "its a 10";
            break;
        case 11:
            cout << "its eleven";
            break;
        case 12:
            cout << "its a dozen";
            break;
        default:
            cout << "its the default !!";
            break;
    }

}

@zander  I post all the info just to see a blank screen !!  Do you see the post above ?

@zander  

Though I've not tried it I think the xcode app has the ability to compile c and c++ as well as swift and uses the CLang compiler to do this.  I've only used the included c++ compiler from the command line.  If you do g++ --help from the command line what do you get?  

If you want a quick test than save the code below as first.cpp and the compile it with:

g++ -o RonTest first.cpp

and you should get an executable (on your mac) call RonTest.

(though before getting in to any c++ malarky you should try python 😀)

SECOND AND LAST TRY - this time the code as text not in the codeblock. (which appears to mean it gets horribly double spaced !!)

------------------------

#include <iostream>

#include <string>

using namespace std;

// first c++

/* and this is writing a 

blooming book comment */

/* to compile - c++ % g++ test1.cpp -o runtest1

   to run - ./runtest1

*/

const int mynum = 28; //note constants must be initialised

int main() {

    int y,z;

    int x = 2;

    string hello = "Hello World "; // note ""

        for (int i = 0; i < hello.length(); i++) {

            cout << hello[i] << endl;

        }

    char myChar = 'B';  // note ''

    cout << "hello world " << x ;

    cout << "\n" << hello << x << "\n";

    y = 5;

    z = y + x;

    cout << "y = " << y << "\n" ;

    cout << "z = " << z;

    cout << " \n myChar = " << myChar << endl;

    cout << "myconst = " << mynum << endl;   

    cout << "Give me a number for x: " << endl;

    cin >> x;

    cout << x << endl;

    cin.clear();

    cin.ignore(1000, '\n'); // ignor upto 1000 character of input if error!

    cout << "Give me a number for y: " << endl;

    cin >> y;

    cout << y << endl;

    z = y + x;

    cout << " x + y = " << z << endl;

    cout << "Give me a string: " << endl;

    cin >> hello;

    cout << "You entered: " << hello << endl;

    /* logical operators

        !   not

        &&  and

        ||  or

    */

    bool ax = false && true;

    bool ay = !true;

    bool az = true || false;

    bool azz = false || true;

    bool azzz = false || false;

    cout << "ax " << ax << endl;

    cout << "ay " << ay << endl;

    cout << "az " << az << endl;

    cout << "azz " << azz << endl;

    cout << "azzz " << azzz << endl;

    if (ax == true){

        cout << "ax is true" << endl;

    }

    if (ay == true){

        cout << "ay is true" << endl;

    } else{

        cout << "ay is false" << endl;

    }

    int mynum = 10;

    if (mynum > 10){

        cout << "mynum is GT 10" << endl;

    } else if (mynum >= 5){

        cout << "mynum is GT 4 but LT 11" << endl;

    } else {

        cout << "mynum is LT5" << endl;

    }

    int a = 8;

    int myArray[a];

    int myArray2[5] = {1,2,3,4,5};

    int myArray1[] = {1,2,3,4};

    int myArray3[] = {1,2,3};

    // int myArray4[];  NO GOOD

    // cout << myArray3[3];  NO GOOD only goes to myArray3[2] just give gibberish but no error!!

    for(int i = 0; i < 10; i++){

        cout << i << endl;

    }

    for (int i = 0; i < sizeof(myArray2)/sizeof(myArray2[0]); i++){

        cout << i << " ";

        cout << myArray2[i] << endl;

    }

    int input = -1;

    while (input != 1 && input !=2){

        cout << "Type a number 1 or 2: ";

        cin >> input;

    }

    input = -1;

    while (true){

        cout << "Type a number 1 or 2: ";

        cin >> input;

        if (input == 1 || input == 2){

            break;

        }

    }

    for (int i = 0; i < 10; i++){

        if (i == 9 || i == 7){

            continue;

        }

        cout << i << endl;

    }

    int myNum = 10;

    do{

        int add;

        cout << "Type a number to add to myNum: ";

        cin >> add;

        myNum += add;

        cout << "myNum is: " << myNum << endl;

    } while (myNum < 10);

    int whatever = 10;

    switch (whatever + 1){

        case 10:

            cout << "its a 10";

            break;

        case 11:

            cout << "its eleven";

            break;

        case 12:

            cout << "its a dozen";

            break;

        default:

            cout << "its the default !!";

            break;

    }

}

@byron No I don't, that has happened to a few of us.

@byron Thanks buddy. Everything worked except it looped endlessly after entering the string. What is interesting is I created it as an ino and it complied but threw the error about no setup or loop. I the did the GCC and it worked as well. I know you are a python guy but I can't wrap my head around most things that are OO, it might have something to do with my brain injury but it doesn't matter. I don't want to burden you with my little project, but can you point me at some resource that will let me traverse a directory tree processing every file found and it is recursive, ie if a dir then change to that dir and do the same thing there etc etc.

@zander

That small c++ code snippet was a first test from a few years back when thought it may be a good idea to learn c++.  But I never used c++ for any real project and and my knowledge of using it has rather faded away.  I've not done much c or arduino c recently either for that matter, though I should probably have a bash at something soon to keep the skills from getting too rusty as I expect it may come in useful if I ever need some really fast and lean code.  Its just that now micopython can be used so well on the likes of the rpi pico and ESP32, and having been using mp for the last few years and its proved fast enough for the sort of things I do, c now seems a bit of a pain and not so enjoyable to code in.   

Which means I don't have any good tips for you in using c++ for your recursive directory tree code.  You will find a goodly number of examples if you google "tree traversal program in c++" or something like that.  You may get more help if you ping that new fellow TFMcCarthy who seems to be an ace at c++ and could probably knock up an OOP example in his sleep 😀 

@byron No problem, I can google up an answer if needed, but it's nice when you can get pointers from friends and people you know who are skilled. I detest OOP, but I recognize some parts are beneficial. My project is a back burner type that I want to document all my code and what source files use what libraries sort of thing. 

FYI if I add an empty setup and loop and name the file into it compiles clean. I am too lazy to hook up a board but I would not be surprised if it executes as well. I will let you know when I get around to trying.

@byron   FYI @zander

But I never used c++ for any real project and my knowledge of using it has rather faded away.

Whereas I never used Python for any real project and my knowledge of using it has rather faded away 🙂

I just downloaded Python (Windows) for a refresh but it didn't come with an editor. I do have it on my RPi 3 computer but I haven't used that for some years now as it was too slow and I need to update to a faster RPi version if I decide to use it again.

I got the FreeBasic code example for finding the intersection of two lines for use in locating an observer's coordinates from angles to visual beacons of known coordinates from,

https://rosettacode.org/wiki/Find_the_intersection_of_two_lines

and I translated it to a C++ version (a previous post above).

However I later noticed rosettacode had a Python version of the line intersection code so I wondered how hard that would be to translate c++.  I found you could do it online.

https://www.codeconvert.ai/c++-to-python-converter

Interestingly I noticed the C++ translated version used a tuple library which I didn't know existed.

#include <tuple>

My c++ translation from the FreeBASIC version didn't need to use tuples.

I am confident that I can translate any of my programs to Python once I refresh my knowledge on its way of doing things and import whatever library is needed.  I have dabbled in a lot of computer languaes although I only used Assembler, FreeBASIC and C++ in any actual projects as I needed low level fast code.

Long live the Tower of Babel of computer languages!  Say the same thing umpteen different ways. Conversion between languages has always interested my ever since my early days when I had to convert between assembler languages with each new PC. (Z80, 6502, 6800, '386)

I found there is also an online c++ to Python converter so I used it to convert my C++ version to Python no tuples involved.

def main():
    x1, y1 = 150, 150
    x2, y2 = 500, 300
    a1, a2 = 63, 164

    x3 = x1 + math.cos(math.radians(a1))
    y3 = y1 + math.sin(math.radians(a1))

    x4 = x2 + math.cos(math.radians(a2))
    y4 = y2 + math.sin(math.radians(a2))

    aa1 = y3 - y1
    b1 = x1 - x3
    c1 = aa1 * x1 + b1 * y1
    aa2 = y4 - y2
    b2 = x2 - x4
    c2 = aa2 * x2 + b2 * y2

    det = aa1 * b2 - aa2 * b1

    if det != 0:
        x5 = (b2 * c1 - b1 * c2) / det
        y5 = (aa1 * c2 - aa2 * c1) / det
    else:
        print("ERROR")
        return

    print(f" x5 = {x5}  y5 = {y5}")

if __name__ == "__main__":
    main()