hack_motor.cpp

00001 #include "mbed.h"
00002 #include "hack_motor.h"
00003 #include "PwmOut.h"
00004 
00005 Wheel::Wheel(PinName M1A_pin, PinName M1B_pin, PinName M2A_pin, PinName M2B_pin)
00006 {
00007     M1A = new PwmOut(M1A_pin);
00008     M2A = new PwmOut(M2A_pin);
00009     M1B = new DigitalOut(M1B_pin);
00010     M2B = new DigitalOut(M2B_pin);
00011     init();
00012 }
00013 
00014 Wheel::~Wheel() // Destructor, cleans up
00015 {
00016     delete M1A;
00017     delete M2A;
00018     delete M1B;
00019     delete M2B;
00020 }
00021 
00022 void Wheel::init() //Initialize the driver pwm to 50Hz
00023 {
00024     M1A->period_ms(20);
00025     M2A->period_ms(20);
00026     speed = 0.0;
00027 }
00028 
00029 void Wheel::set_speed(float speed)
00030 {
00031     this->speed = speed;
00032 }
00033 
00034 float Wheel::get_speed()
00035 {
00036     return speed;
00037 }
00038 
00039 void Wheel::FW()
00040 {
00041     fw = speed;
00042     M1A->write(fw); //Set the duty cycle to the wanted percent, from speed variable
00043     M2A->write(fw); // -//-
00044     *M1B = 0;
00045     *M2B = 0;
00046     wait_ms(1);
00047 }
00048 
00049 void Wheel::FW(float right, float left) // set the actual speed for right, left motor
00050 {   
00051     M1A->write(right); //Set the duty cycle to the wanted percent, from speed variable
00052     M2A->write(left); // -//-
00053     *M1B = 0;
00054     *M2B = 0;
00055     wait_ms(1); 
00056 }
00057 
00058 void Wheel::BW()
00059 {
00060     bw = 1 - speed;
00061     M1A->write(bw); //Set the duty cycle to the wanted percent, from speed variable
00062     M2A->write(bw); // -//-
00063     *M1B = 1;
00064     *M2B = 1;
00065 }
00066 
00067 void Wheel::right()
00068 {
00069     /*    M1A->write(0.75f * speed);
00070         M2A->write(1 - 0.25f * speed);
00071         *M1B = 0;
00072         *M2B = 1;
00073         */
00074 
00075     M1A->write(speed);
00076     M2A->write(0.0f);
00077     *M1B = 0;
00078     *M2B = 0;
00079 }
00080 
00081 void Wheel::left()
00082 {
00083     /*
00084      M1A->write(1 - 0.25f * speed);
00085      M2A->write(0.75f * speed);
00086      *M1B = 1;
00087      *M2B = 0;
00088      */
00089     M1A->write(0.0f);
00090     M2A->write(speed);
00091     *M1B = 0;
00092     *M2B = 0;
00093 }
00094 
00095 void Wheel::stop()
00096 {
00097     M1A->write(0.0f);
00098     M2A->write(0.0f);
00099     *M1B = 0;
00100     *M2B = 0;
00101 }