LOCOMOTION.h

00001 #ifndef LOCOMOTION_H
00002 #define LOCOMOTION_H
00003 
00004 #include "mbed.h"
00005 #include "LOCALIZE.h"
00006 
00007 #define SPEED_TURN_MIN  0.20
00008 #define SPEED_TURN_MAX  0.65
00009 
00010 enum {
00011     ANGLE_TURN  = 0,
00012     ANGLE_BIAS  = 1,
00013 };
00014 
00015 class LOCOMOTION
00016 {
00017 public:
00018     LOCOMOTION(PinName motor1F, PinName motor1B,PinName motor2F, PinName motor2B, PinName forward1, PinName forward2);
00019     PwmOut _m1f;
00020     PwmOut _m1b;
00021     PwmOut _m2f;
00022     PwmOut _m2b;
00023     DigitalOut _m1dir;
00024     DigitalOut _m2dir;
00025     bool setXPos(int target, int current, int error, int angle);
00026     bool setYPos(int target, int current, int error, int angle);
00027     bool setAngle(int target, int current, int error, int mode);
00028     int wrap(int num);
00029 
00030 protected:
00031     float s;
00032 };
00033 #endif