File content as of revision 2:b996c95912b5:

#include "mbed.h"
#include "hack_motor.h"
#include "PwmOut.h"

Wheel::Wheel() : M1A(PTC8), M1B(PTC10), M2A(PTC9), M2B(PTC11), GrabA(PTA5), GrabB(PTC7), HejsA(PTA4), HejsB(PTC5)
    {
        init();    
    }
    
    
void Wheel::init() //Initialize the driver pwm to 150Hz
    {
        M1A.period(0.0066);
        M2A.period(0.0066);
        GrabA.period(0.0066);
        HejsA.period(0.0066);
        speed = 0.0;
    }    
    
void Wheel::FW()
    {
        fw = 0.5+(0.5*speed); //forward lies in the upper 50% of the duty cycle
        M1A.write(fw); //Set the duty cycle to the wanted percent, from speed variable
        M2A.write(fw); // -//-
        M1B = 0;
        M2B = 0;
        wait_ms(1);
    }
    
void Wheel::BW()
    {
        bw = 0.5-(0.5*speed); //Backward lies within the lower 50% of the duty cycle
        M1A.write(bw); //Set the duty cycle to the wanted percent, from speed variable
        M2A.write(bw); // -//-
        M1B = 1; 
        M2B = 1;
    }
    
void Wheel::right()
    {
        M1A.write(0.75); //Left side forward 50%
        M2A.write(0.25); //Right side backwards 50%
        M1B = 0;
        M2B = 1;
    }
    
void Wheel::left()
    {
        M1A.write(0.25); //Right side forward 50%
        M2A.write(0.75); //Left side backwards 50%
        M1B = 1;
        M2B = 0;
    }
    
void Wheel::stop()
    {
        M1A.write(0.0); //Pin A's set low
        M2A.write(0.0);
        M1B = 0;
        M2B = 0;  //Pin B's set high
        GrabA.write(0.0);
        HejsA.write(0.0);
        GrabB = 0;
        HejsB = 0;
    }
    
void Wheel::open()
    {
        GrabA.write(0.3);
        GrabB = 1;
        wait(0.1);
        GrabA.write(0.0);
        GrabB = 0;
    }
    
void Wheel::close()
    {
        GrabA.write(0.8);
        GrabB = 0;
        wait(0.1);
        GrabA.write(0.0);
        GrabB = 0;
    }
    
    void Wheel::hejs()
    {
        HejsA.write(0.7);
        HejsB = 0;
        wait(0.1);
        HejsA.write(0.0);
        HejsB = 0;
        
    }
    
void Wheel::saenk()
    {
        HejsA.write(0.25);
        HejsB = 1;
        wait(0.1);
        HejsA.write(0.0);
        HejsA = 0;
        }
        
void Wheel::venSelv1() // Højre
    {
        M1A.write(0.7); //Set the duty cycle to the wanted percent, from speed variable
        M2A.write(0.0); // -//-
        M1B = 0;
        M2B = 0;
        wait_ms(100);
        M2A.write(0.7);
    }
void Wheel::venSelv2() // Venstre
    {
        M1A.write(0.0); //Set the duty cycle to the wanted percent, from speed variable
        M2A.write(0.7); // -//-
        M1B = 0;
        M2B = 0;
        wait_ms(100);
        M1A.write(0.7);
    }
void Wheel::venSelv3() // Skarp højre
    {
        M1A.write(0.8);
        M2A.write(0.8);
        wait_ms(25);
        M1A.write(0.7); //Set the duty cycle to the wanted percent, from speed variable
        M2A.write(0.95); // -//-
        M1B = 0;
        M2B = 1;
        wait_ms(10);
        M1A.write(0.0);
    }
void Wheel::venSelv4() // Venstre Skarpt
    {
        M1A.write(0.8);
        M2A.write(0.8);
        wait_ms(25);
        M1A.write(0.95); //Set the duty cycle to the wanted percent, from speed variable
        M2A.write(0.7); // -//-
        M1B = 1;
        M2B = 0;
        wait_ms(10);
        M1A.write(0.0);
    }