Drive wheeled robot platform

Dependencies:   mbed

Fork of mbed_blinky by Mbed

main.cpp

Committer:
RichardAmes
Date:
2017-09-25
Revision:
9:ada1a8a81354
Parent:
8:840b3b80cfa2

File content as of revision 9:ada1a8a81354:

#include "mbed.h"

DigitalOut drive_led(LED2);
PwmOut motorA(PTD0);
PwmOut motorB(PTD5);
DigitalOut dirB1(PTA13);
DigitalOut dirB2(PTC9);
DigitalOut dirA2(PTC8);
DigitalOut dirA1(PTA5);

/* set pulse width for motor drive signals */
void drive_init(void)
{
    motorA.period(0.01f);
    motorB.period(0.01f);
}

/* set signals to move motors */
/* bias min c. 25, max 100 */
void move(int bias_a, int bias_b, int milliseconds)
{
    if ((bias_a == 0) && (bias_b == 0))
        drive_led = 1;
    else
        drive_led = 0;
    if (bias_a < 0)
    {
        dirA1 = 0;
        dirA2 = 1;
    }
    else if (bias_a > 0)
    {
        dirA1 = 1;
        dirA2 = 0;
    }
    else
    {
        dirA1 = 1;
        dirA2 = 1;
    }
    if (bias_b < 0)
    {
        dirB1 = 1;
        dirB2 = 0;
    }
    else if (bias_b > 0)
    {
        dirB1 = 0;
        dirB2 = 1;
    }
    else
    {
        dirB1 = 1;
        dirB2 = 1;
    }
    motorA.write((float)abs(bias_a) / 100);
    motorB.write((float)abs(bias_b) / 100);
    wait_ms(milliseconds);
}

int main()
{
    int i;
        
    drive_init();
    
    for (i = 0; i < 4; i++)
    {
        move(0, 0, 500);
        move(-50, 50, 1000);
        move(0, 0, 500);
        move(50, -50, 1000);
    }
    while(1)
    {
        move(0, 0, 500);
    }
}