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     
00052     M1A->write(right); //Set the duty cycle to the wanted percent, from speed variable
00053     M2A->write(left); // -//-
00054     *M1B = 0;
00055     *M2B = 0;
00056     wait_ms(1); 
00057 }
00058 
00059 void Wheel::BW()
00060 {
00061     bw = 1 - speed;
00062     M1A->write(bw); //Set the duty cycle to the wanted percent, from speed variable
00063     M2A->write(bw); // -//-
00064     *M1B = 1;
00065     *M2B = 1;
00066 }
00067 
00068 void Wheel::right()
00069 {
00070     /*    M1A->write(0.75f * speed);
00071         M2A->write(1 - 0.25f * speed);
00072         *M1B = 0;
00073         *M2B = 1;
00074         */
00075 
00076     M1A->write(speed);
00077     M2A->write(0.0f);
00078     *M1B = 0;
00079     *M2B = 0;
00080 }
00081 
00082 void Wheel::left()
00083 {
00084     /*
00085      M1A->write(1 - 0.25f * speed);
00086      M2A->write(0.75f * speed);
00087      *M1B = 1;
00088      *M2B = 0;
00089      */
00090     M1A->write(0.0f);
00091     M2A->write(speed);
00092     *M1B = 0;
00093     *M2B = 0;
00094 }
00095 
00096 void Wheel::stop()
00097 {
00098     M1A->write(0.0f);
00099     M2A->write(0.0f);
00100     *M1B = 0;
00101     *M2B = 0;
00102 }