File content as of revision 1:d4fa7e5e42cc:

#include "mbed.h"
#include "QEI.h"
#include "PID.h"
#include "Motor.h"
#include "BrocketJacobian.h"
#include "ReceiverIR.h"
#include "Servo.h"
#include "emg.h"
#include "filter.h"

// Declare constants etc
float Interval=0.01f;   
float pi=3.14159265359;

// Declare Analogin in for Potmeter, Can be used for references.
AnalogIn PotMeter1(A0);
AnalogIn PotMeter2(A1);

// Declare IR receiver
ReceiverIR ir_rx(D2);
 
// Declare Servo
//Servo ZServo(A5);
PwmOut ZServo(A5);
// Declare Magnet
DigitalOut Magnet(D0);
 
// Declare motor objects that will control the motor
Motor Motor1(D5,D4,D12,D13,D9,Interval);
Motor Motor2(D6,D7,D10,D11,D8,Interval);

// Declare EMG shields and variables
emg_shield emg1(A2,500);
emg_shield emg2(A3,500);
emg_shield emg3(A4,500);
bool EMG_Direction = 0;
InterruptIn DirectionButton(NC); // Switch 2

// Declare tickers
Ticker ControlTicker;
Ticker GetRefTicker;
Ticker GotoPosTicker;
 
// Delare the GYset and GXset, which are the positions derived from integrating
// after the applying the jacobian inverse
float GXset = 40.5;    //from EMG in cm
float GYset = 11;    //from EMG in cm 

// Motor angles in radialen
float q1set = 0.29f+0.1f;
float q2set = (3.3715f-2.0f*pi)+0.1f;
 
float x_dot_remote = 0.0f;
float y_dot_remote = 0.0f;
 
// Declare stuff for the IR receiver
RemoteIR::Format format;
uint8_t buf[4];


bool EMG_CONTROL=0;


// Declare serial object for setting the baudrate
RawSerial pc(USBTX,USBRX);

void DirectionButtonPressed(){
    EMG_Direction=!EMG_Direction;
}

// Looping function using the Jacobian transposed
void LoopFunctionJacTransposed()
{
    float q1 = Motor1.GetPos();
    float q2 = Motor2.GetPos();
    
    //start values
    float tau1 = 0.0f;             //previous values are irrelevant
    float tau2 = 0.0f;             //previous values are irrelevant
 
    float Xcurr = 0.0f;            //new value is calculated, old replaced
    float Ycurr = 0.0f;            //new value is calculated, old replaced
    
    float b = 200.0f;          //damping
    float k = 1.0f;         //stiffness of the spring pulling on the end effector
    
    // Call function to calculate Xcurr and Ycurr from q1 and q2
    Brocket(q1, q2, Xcurr, Ycurr);
        
    // Compute spring forces 
    float Fx = k*(GXset-Xcurr);
    float Fy = k*(GYset-Ycurr);
    
    // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset
    TransposeJacobian(q1, q2, Fx, Fy, tau1, tau2); 
    
    // torque to joint velocity
    float omg1 = tau1/b;
    float omg2 = tau2/b;
    
    printf("%f %f\r\n",GXset,GYset);

    // joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2
    q1set = q1set + omg1*Interval;
    q2set = q2set + omg2*Interval;
    if (q1set>(0.75f*pi))   {q1set=(0.75f*pi);}
    if (q1set<0.30f)        {q1set = 0.30f;}
    if (q2set>0.0f)         {q2set=(0.0f);}
    if (q2set<-2.79f)        {q2set = -2.79f;}
    
    // Call the function that controlls the motors
    Motor1.GotoPos(q1set);
    Motor2.GotoPos(q2set);
    //printf("%f %f\r\n",q1set,q2set);
}

void GotoPosition(float x, float y){
    float error =10.0f;
    LowPass RefLowPass1;
    LowPass RefLowPass2;
    RefLowPass1.a=0.99;
    RefLowPass2.a=0.99;
    
    float Xcurr; float Ycurr;
    Brocket(Motor1.GetPos(), Motor2.GetPos(), Xcurr, Ycurr);
    
    RefLowPass1.yprev=Xcurr;
    RefLowPass2.yprev=Ycurr;
    
    GXset=RefLowPass1.filter(x);
    GYset=RefLowPass2.filter(y);
    
    GotoPosTicker.attach(&LoopFunctionJacTransposed,Interval);
    while (error>5.0f){
        GXset=RefLowPass1.filter(x);
        GYset=RefLowPass2.filter(y);
        Brocket(Motor1.GetPos(), Motor2.GetPos(), Xcurr, Ycurr);
        error = sqrt(pow(Xcurr-x,2)+pow(Ycurr-y,2));
        wait(0.1);
    }
    GotoPosTicker.detach();
}

void LoopJacInverse(){
    // Get Motor Positions
    float q1 = Motor1.GetPos();
    float q2 = Motor2.GetPos();
    
    // Define velocities for control
    float q1_dot=0.0f;
    float q2_dot=0.0f;
    
    float vx=0.0f;
    float vy=0.0f;
    
    if (EMG_CONTROL){
        // Get the velocities from EMG
        float ScaleFactor = 200.0f;
        vx=ScaleFactor*(emg1.GetValue()- 2.0f*emg2.GetValue());
        vy=0.0f;//sclaeFactor*(emg2.GetValue());
    }
    
    
    if ( !(x_dot_remote ==0.0f)){
        vx=x_dot_remote;
    }
    if ( !(y_dot_remote == 0.0f)){
        vy=y_dot_remote;
    }
    
    // Apply Jacobian Inverse
    InverseJacobian(q1,q2,vx,vy,q1_dot,q2_dot);
    
    // Integrate q1dot and q2dot to obtain positions
    q1set += q1_dot*Interval;
    q2set += q2_dot*Interval;
    
    if (q1set>(0.75f*pi))   {q1set=(0.75f*pi);}
    if (q1set<0.30f)        {q1set = 0.30f;}
    if (q2set>0.0f)         {q2set=(0.0f);}
    if (q2set<-2.79f)        {q2set = -2.79f;}
    
    // Call the motor control functions
    Motor1.GotoPos(q1set);
    Motor2.GotoPos(q2set);
}
 
 
// Start homing the motors
void HomingLoop(){
    // with param:(Direction , PWM , Home pos in radians)
    ZServo.write(0.1f);       // Varying the servo pwm between 2.5 and 10 % sends it between 0 and 180 degrees
    wait(1);
    Motor1.Homing(1,0.1f,0.2879f);
    Motor2.Homing(1,0.15f,-2.897f);
}

// Function for picking up a checkers playthingy
void PickUp(){
    ZServo.write(0.035f);       // Varying the servo pwm between 2.5 and 10 % sends it between 0 and 180 degrees
    wait(1);
    Magnet=1;
    ZServo.write(0.1f);
    wait(1);
}

// Function for dropping a checkers playthingy
void LayDown(){
    //ZServo = 0.0f;
    ZServo.write(0.035f);       // Varying the servo pwm between 2.5 and 10 % sends it between 0 and 180 degrees
    wait(1);
    Magnet=0;

    ZServo.write(0.1f);
    wait(1);
}

// Forward declarate remote controller function
void RemoteController();
 
 
// Give Reference Position
void DeterminePosRef(){
    //GXset=40*PotMeter1.read(); // Reference in Rad
    //GYset=40*PotMeter2.read(); // Reference in Rad
    //printf("Set: %f %f",GXset,GYset);
}
 
int main() {
    //ZServo.calibrate(0.0005,120);
    ZServo.period_ms(20);               // Servo pwm interval should be 50 Hz
    pc.baud(115200);
    pc.printf("Program BIOROBOTICS startup\r\n");
    q1set = 0.25f*pi;
    q2set = -0.75f*pi;
    
    // Define Controller Values
    Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
    Motor2.SetInputLimits(-2.0f*pi,2.0f*pi);
    
    Motor1.SetOutputLimits(-0.3f,0.3f);
    Motor2.SetOutputLimits(-0.8f,0.8f);
    
    Motor1.SetPID(50.0f,10.0f,0.001f);
    Motor2.SetPID(50.0f,10.0f,0.001f);
    
    Motor1.SetGearRatio(3.0f);
    Motor2.SetGearRatio(1.8f);
  
    // Start homing function
    pc.printf("Homing \r\n");
    HomingLoop();
    
    // Start Tickers
    pc.printf("Starting Tickers \r\n");
    ControlTicker.attach(&LoopJacInverse,Interval);
    DirectionButton.rise(&DirectionButtonPressed);
 
    // Check wheater a remote command has been send
    while(1)
    { 
        if (ir_rx.getState() == ReceiverIR::Received)
        {
            pc.printf("received");
            
            
            RemoteController();
            wait(0.1);
        }
    }       
    
    
    while(1){}
 
 
}


 
void RemoteController(){
    
   int bitcount;
 
 
   bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
    int state = buf[2];
    switch(state)
    {
        case 22: //1
            pc.printf("1\n\r");
            ControlTicker.attach(&LoopJacInverse,Interval);
            break;
            
        case 25: //1
            pc.printf("2\n\r");
            ControlTicker.detach();
            Motor1.Stop();
            Motor2.Stop();
            PickUp();
            ControlTicker.attach(&LoopJacInverse,Interval);
            break;
            
        case 13: //1                
            pc.printf("3\n\r");
            
            break;
            
        case 12: //1
            pc.printf("4\n\r");
            ControlTicker.detach();
            Motor1.Stop();
            Motor2.Stop();
            GotoPosition(0.0f,0.0f);
            break;
            
        case 24: //1
            pc.printf("5\n\r");
            break;
        case 94: //1
            pc.printf("6\n\r");
            ControlTicker.detach();
            Motor1.Stop();
            Motor2.Stop();
            break;
        case 8: //1
            pc.printf("7\n\r");
            break;
        case 28: //1
            pc.printf("8\n\r");
            ControlTicker.detach();
            Motor1.Stop();
            Motor2.Stop();
            LayDown();
            ControlTicker.attach(&LoopJacInverse,Interval);
            break;
        case 90: //1
            pc.printf("9\n\r");
            break;
        case 70: //1
            pc.printf("Boven\n\r");
            y_dot_remote = 5.0f;
            break;
        case 21: //1
            pc.printf("Onder\n\r");
            y_dot_remote = -5.0f;
            break;
        case 68: //1
            pc.printf("Links\n\r");
            x_dot_remote = -5.0f;
            break;
        case 67: //1
            pc.printf("Rechts\n\r");
            x_dot_remote = 5.0f;
            break;
        case 64: //1
            pc.printf("OK\n\r");
            x_dot_remote = 0.0f;
            y_dot_remote = 0.0f;
            break;
        default:
            break;
    }
}