Hi guys, so I want 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 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 and a nucleo-f401re chip.
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 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 which I have no clue how to do. 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);
- }
- }