hack_motor.cpp

00001 #include "mbed.h"
00002 #include "hack_motor.h"
00003 #include "PwmOut.h"
00004 
00005 Wheel::Wheel() : M1A(PTC8), M1B(PTC10), M2A(PTC9), M2B(PTC11), GrabA(PTA5), GrabB(PTC7), HejsA(PTA4), HejsB(PTC5)
00006     {
00007         init();    
00008     }
00009     
00010     
00011 void Wheel::init() //Initialize the driver pwm to 150Hz
00012     {
00013         M1A.period(0.0066);
00014         M2A.period(0.0066);
00015         GrabA.period(0.0066);
00016         HejsA.period(0.0066);
00017         speed = 0.0;
00018     }    
00019     
00020 void Wheel::FW()
00021     {
00022         fw = 0.5+(0.5*speed); //forward lies in the upper 50% of the duty cycle
00023         M1A.write(fw); //Set the duty cycle to the wanted percent, from speed variable
00024         M2A.write(fw); // -//-
00025         M1B = 0;
00026         M2B = 0;
00027         wait_ms(1);
00028     }
00029     
00030 void Wheel::BW()
00031     {
00032         bw = 0.5-(0.5*speed); //Backward lies within the lower 50% of the duty cycle
00033         M1A.write(bw); //Set the duty cycle to the wanted percent, from speed variable
00034         M2A.write(bw); // -//-
00035         M1B = 1; 
00036         M2B = 1;
00037     }
00038     
00039 void Wheel::right()
00040     {
00041         M1A.write(0.75); //Left side forward 50%
00042         M2A.write(0.25); //Right side backwards 50%
00043         M1B = 0;
00044         M2B = 1;
00045     }
00046     
00047 void Wheel::left()
00048     {
00049         M1A.write(0.25); //Right side forward 50%
00050         M2A.write(0.75); //Left side backwards 50%
00051         M1B = 1;
00052         M2B = 0;
00053     }
00054     
00055 void Wheel::stop()
00056     {
00057         M1A.write(0.0); //Pin A's set low
00058         M2A.write(0.0);
00059         M1B = 0;
00060         M2B = 0;  //Pin B's set high
00061         GrabA.write(0.0);
00062         HejsA.write(0.0);
00063         GrabB = 0;
00064         HejsB = 0;
00065     }
00066     
00067 void Wheel::open()
00068     {
00069         GrabA.write(0.3);
00070         GrabB = 1;
00071         wait(0.1);
00072         GrabA.write(0.0);
00073         GrabB = 0;
00074     }
00075     
00076 void Wheel::close()
00077     {
00078         GrabA.write(0.8);
00079         GrabB = 0;
00080         wait(0.1);
00081         GrabA.write(0.0);
00082         GrabB = 0;
00083     }
00084     
00085     void Wheel::hejs()
00086     {
00087         HejsA.write(0.7);
00088         HejsB = 0;
00089         wait(0.1);
00090         HejsA.write(0.0);
00091         HejsB = 0;
00092         
00093     }
00094     
00095 void Wheel::saenk()
00096     {
00097         HejsA.write(0.25);
00098         HejsB = 1;
00099         wait(0.1);
00100         HejsA.write(0.0);
00101         HejsA = 0;
00102         }
00103         
00104 void Wheel::venSelv1() // Højre
00105     {
00106         M1A.write(0.7); //Set the duty cycle to the wanted percent, from speed variable
00107         M2A.write(0.0); // -//-
00108         M1B = 0;
00109         M2B = 0;
00110         wait_ms(100);
00111         M2A.write(0.7);
00112     }
00113 void Wheel::venSelv2() // Venstre
00114     {
00115         M1A.write(0.0); //Set the duty cycle to the wanted percent, from speed variable
00116         M2A.write(0.7); // -//-
00117         M1B = 0;
00118         M2B = 0;
00119         wait_ms(100);
00120         M1A.write(0.7);
00121     }
00122 void Wheel::venSelv3() // Skarp højre
00123     {
00124         M1A.write(0.8);
00125         M2A.write(0.8);
00126         wait_ms(25);
00127         M1A.write(0.7); //Set the duty cycle to the wanted percent, from speed variable
00128         M2A.write(0.95); // -//-
00129         M1B = 0;
00130         M2B = 1;
00131         wait_ms(10);
00132         M1A.write(0.0);
00133     }
00134 void Wheel::venSelv4() // Venstre Skarpt
00135     {
00136         M1A.write(0.8);
00137         M2A.write(0.8);
00138         wait_ms(25);
00139         M1A.write(0.95); //Set the duty cycle to the wanted percent, from speed variable
00140         M2A.write(0.7); // -//-
00141         M1B = 1;
00142         M2B = 0;
00143         wait_ms(10);
00144         M1A.write(0.0);
00145     }