Maker Pro
Maker Pro

help with code programming robot to drive in a rectangle using C

HappyC4mper

Oct 13, 2017
1
Joined
Oct 13, 2017
Messages
1
Hi guys, so I have this project where I have to code a robot to drive around in a rectangle. Now, the problem is, I've had no experience in coding so I had to quickly learn the basics of C programming. I'm using a program called keil uvision 5 to code and a terminal called putty to see my values.
I just need some guidance in terms of coding this mini project. At the moment I've managed to code it to go in a rectangle however, I'm not too sure how to make it turn 90 degrees accurately. I'm using a nucleo-f401re microcontroller, L149 motor with hall effect sensors (A1,A2,B1,B2(2 in each motor 90 degrees apart)), the Motors are 4.5VDC Nominal and 20.8:1 gear ratio.
For each hall effect sensors Im expecting 3 pulses per revolution of the motor shaft. I've calculated the circumference of my wheels which is 173mm. Now for the width of the square, I need it to go 1meter. Now to get the number of pulses to reach one meter I did, 1000/173 = 5.79 (which is the no. of resolution). Now taking into account of the gearbox its 20.8x3(no. pulses per rev) = 62.4 ---> times it by the number of resolution I get 361 pulses to reach one meter. I just half that for the width of the rectangle which is 181 pulses. Now the problem is I'm not sure how to make 90 degree turns accurately :/
the hardest part for me is to adjust the speed dynamically to maintain a constant speed of 1 revolution per second. I know I should control each wheel independently as they will have slightly different characteristics.
If someone could explain how that bit works, that would be great!
here is my code so far:


Code:
#include "mbed.h"
 
//Status LED
    DigitalOut led(LED1);
 
//Motor PWM (speed)
    PwmOut PWMA(PA_8);
    PwmOut PWMB(PB_4);
 
//Motor Direction
    DigitalOut DIRA(PA_9);
    DigitalOut DIRB(PB_10);
 
//Hall-Effect Sensor Inputs
    DigitalIn HEA1(PB_2);
    DigitalIn HEA2(PB_1);
    DigitalIn HEB1(PB_15);
    DigitalIn HEB2(PB_14);
 
//On board switch
    DigitalIn SW1(USER_BUTTON);
 
 
//Timer used for measuring speeds
    Timer timer;
 
//Enumerated types
    enum DIRECTION   {FORWARD=0, REVERSE};
    enum PULSE       {NOPULSE=0, PULSE};
    enum SWITCHSTATE {PRESSED=0, RELEASED};
 
//Debug GPIO
    DigitalOut probe(D10);
 
//Duty cycles
    float dutyA = 1.0f; //100%
    float dutyB = 1.0f; //100%
 
//prototypes
    void sense_count_long(void);
    void sense_count_short(void);
    void sense_count_turn(void);
    void sense_count_victory(void);
    void longStraight(void);
    void rightTurn(void);
    void shortStraight(void);
    void victory(void);
    void stop(void);
 
 
 
 
 
 
int main()
{
 
    //Set initial motor direction to forward
    DIRA = FORWARD;
    DIRB = FORWARD;
 
    //Set motor period to 100Hz ( 1/T )
    PWMA.period_ms(10);
    PWMB.period_ms(10);
 
    //Set initial motor speed to 0% duty
    PWMA.write(0.0f);     
    PWMB.write(0.0f);     
 
    //Wait for USER button (blue pull-down switch) to start
    printf("Press USER button to start\n\r");
    led = 0;
    while (SW1 == RELEASED);
    led = 1;
  
  
        longStraight();
        rightTurn();
        shortStraight();
        rightTurn();
        longStraight();
        rightTurn();
        shortStraight();
        rightTurn();
        victory();
        stop();
  
 
                }
  
 
  
//    //Array of sensor data
//    int tA1[2];
//    int tA2[2];
 
//    //Instructions to user
//    printf("Press USER button to adapt duty (to convernge on 1 rotation/s)\n\r");
 
//    //Main polling loop
//    while(1) {
//        //Reset timer and Start
//        timer.reset();
//        timer.start();
 
//        //Wait for HEA1 to go to LOW
//        while (HEA1 == PULSE);
 
//        //Wait for rising edge of A1 and log time
//        while (HEA1 == NOPULSE);
//        tA1[0] = timer.read_us();
 
//        //Wait for rising edge of A2 and log time (30 degrees?)
//        while (HEA2 == NOPULSE);
//        tA2[0] = timer.read_us();
 
 
//        //Wait for falling edge of A1
//        while (HEA1 == PULSE);
//        //Wait for falling edge of A2
//        while (HEA2 == PULSE);
 
//        //Wait for rising edge of A1 and log time
//        while (HEA1 == NOPULSE);
//        tA1[1] = timer.read_us();
 
//        //Wait for rising edge of A2 and log time (30 degrees?)
//        while (HEA2 == NOPULSE);
//        tA2[1] = timer.read_us();
 
//                printf("tA1(0) = %d\n", tA1[0]);
//        printf("tA1(1) = %d\n", tA1[1]);
//        printf("tA2(0) = %d\n", tA2[0]);
//                printf("tA2(1) = %d\n", tA2[1]);
 
//        //Calculate the frequency of rotation
//        float fA1 = 1.0f/(( tA1[1]-tA1[0] )*(float)3.0E-6);
//        float fA2 = 1.0f/(( tA2[1]-tA2[0] )*(float)3.0E-6);
//        float fA = (fA1 + fA2)*0.5f;    //Average frequency
//                printf("A1 Shaft: %6.2fHz \t Wheel: %6.2f\n", fA1, fA1/20.2f);
//                printf("A2 Shaft: %6.2fHz \t Wheel: %6.2f\n", fA2, fA2/20.2f);
//                printf("Average A2 Shaft: %6.2fHz \t Wheel: %6.2f\n", fA, fA/20.2f);
 
//        //Reset timers
//        timer.stop();
 
//        //Wait for button press
//        while (SW1 == 1);
//        wait(0.2);
//        while (SW1 == 0);
//        wait(0.1);
 
//        //Adapt duty to meet 1 revolution per second
//        float wA = fA/20.2f;                    //Wheel speed
//        float deltaA = 1.0f-wA;             //Error
//        dutyA = dutyA + deltaA*0.1f;    //Increase duty in proportion to the error
//        //The max and min values of duty and 0.0 and 1.0 respectively
//        dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
//                dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
//        PWMA.write(dutyA);                  //Update duty cycle
 
//        //Echo to the terminal
//        printf("Adapting duty cycle to %6.2f\n", dutyA);
 
//
//    }
 
 
 
    void longStraight(void){
            PWMA.write(1.0f);     
            PWMB.write(0.97f);
            sense_count_long();
 
}
 
    void shortStraight(void) {
            PWMA.write(1.0f);     
            PWMB.write(0.97f);
        sense_count_short();
 
}
 
    void rightTurn(void){
            PWMA.write(0.8f);     
            PWMB.write(0.0f);
            sense_count_turn();
}
 
    void victory(void){
            PWMA.write(0.0f);     
            PWMB.write(1.0f);
            sense_count_victory();
  
      
}
 
    void stop(void){
            PWMA.write(0.0f);
            PWMB.write(0.0f);
    }   
 
 
    void sense_count_long(void){
                int count=0;
                while(count<370){
                   while(HEA1 == PULSE){}
                while(HEA2 == PULSE){}
                while(HEA1 == NOPULSE){}
                while(HEA2 == NOPULSE){}
                count++;
                        printf("count = %d\n\r", count); //debug only ms!!!
                    }
 
              
                }
 
    void sense_count_short(void){
                int count=0;
                while(count<205){
                  while(HEA1 == PULSE){}
                while(HEA2 == PULSE){}
                while(HEA1 == NOPULSE){}
                while(HEA2 == NOPULSE){}
                count++;
                        printf("count = %d\n\r", count); //debug only ms!!!
                    }
                }
 
    void sense_count_turn(void){
                int count=0;
                while(count<70){
                while(HEA1 == PULSE){}
                while(HEA2 == PULSE){}
                while(HEA1 == NOPULSE){}
                while(HEA2 == NOPULSE){}
                count++;
                    printf("count = %d\n\r", count); //debug only;
                    }
                }
 
    void  sense_count_victory(void){
              
                    for(int count = 0; count<280; count++){
                    while(HEB2 == PULSE){}
                    while(HEB2 == NOPULSE){}
              
                            printf("count = %d\n\r", count);
                      
                        }
              
                    }
 
Top