A simple program to check the operation of the motors

Dependencies:   mbed

MotorSystemTest.cpp

Committer:
Hypna
Date:
2014-10-24
Revision:
0:9dba1f8fc770

File content as of revision 0:9dba1f8fc770:

#include "mbed.h"

#define MOTOR_SCALE 0.25

void forward();
void backward();
void left();
void right();

PwmOut whl1Spd(PTB0);
PwmOut whl2Spd(PTB1);
PwmOut whl3Spd(PTB2);
PwmOut whl4Spd(PTB3);
    
DigitalOut whl1Dir(PTE20);
DigitalOut whl2Dir(PTE21);
DigitalOut whl3Dir(PTE22);
DigitalOut whl4Dir(PTE23);

int main()
{
    for(int i = 0; i < 10; i++)
    {
        forward();
        wait(1);
        backward();
        wait(1);
        left();
        wait(1);
        right();
        wait(1);
    }
    
    return 0;
}

void forward()
{
    whl1Dir = whl2Dir = whl3Dir = whl4Dir = 1;
    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 1*MOTOR_SCALE;
    wait(2);
    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 0;
}

void backward()
{
    whl1Dir = whl2Dir = whl3Dir = whl4Dir = 0;
    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 1*MOTOR_SCALE;
    wait(2);
    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 0; 
}

void left()
{
    whl2Dir = whl4Dir = 1;
    whl1Dir = whl3Dir = 0;
    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 1*MOTOR_SCALE;
    wait(2);
    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 0;
   
}

void right()
{
    whl2Dir = whl4Dir = 0;
    whl1Dir = whl3Dir = 1;
    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 1*MOTOR_SCALE;
    wait(2);
    whl1Spd = whl2Spd = whl3Spd = whl4Spd = 0;
}