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

#### HappyC4mper

Oct 13, 2017
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;
//    int tA2;

//    //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);

//        //Wait for rising edge of A2 and log time (30 degrees?)
//        while (HEA2 == NOPULSE);

//        //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);

//        //Wait for rising edge of A2 and log time (30 degrees?)
//        while (HEA2 == NOPULSE);

//                printf("tA1(0) = %d\n", tA1);
//        printf("tA1(1) = %d\n", tA1);
//        printf("tA2(0) = %d\n", tA2);
//                printf("tA2(1) = %d\n", tA2);

//        //Calculate the frequency of rotation
//        float fA1 = 1.0f/(( tA1-tA1 )*(float)3.0E-6);
//        float fA2 = 1.0f/(( tA2-tA2 )*(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);

}

}

J
Replies
1
Views
2K
R
Replies
0
Views
36
Replies
3
Views
547
Replies
4
Views
778
Replies
10
Views
3K