File content as of revision 0:df93928b266c:

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

// Declare constants etc
float Interval=0.02f;
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 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(A0,500);
emg_shield emg2(A1,500);
emg_shield emg3(A2,500);
bool EMG_Direction = 0;
InterruptIn DirectionButton(D6); // Switch 2

// Declare tickers
Ticker ControlTicker;
Ticker GetRefTicker;
 
// 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 
 
// Constant robot parameters
const float L1 = 27.5f;      //length arm 1 in cm
const float L2 = 32.0f;        //length arm 2 in cm
 
// Motor angles in radialen
float q1set = 0.25f*pi;
float q2set = -0.5f*pi;
 
// Declare stuff for the IR receiver
RemoteIR::Format format;
uint8_t buf[4];

// 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 = 0.05f;         //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
    InverseJacobian(q1, q2, Fx, Fy, tau1, tau2); 
    
    // torque to joint velocity
    float omg1 = tau1/b;
    float omg2 = tau2/b;

    // 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;
    
    // Call the function that controlls the motors
    Motor1.GotoPos(q1set);
    Motor2.GotoPos(q2set);
}


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;
    
    // Get the velocities from EMG
    float vx=0.0f;
    float vy=0.0f;
    
    // 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;
    
    // 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)
    Motor1.Homing(1,0.2f,0.29f);
    Motor2.Homing(1,0.1f,(3.3715f-2.0f*pi));
}

// Function for picking up a checkers playthingy
void PickUp(){
 
}

// Function for dropping a checkers playthingy
void LayDown(){
    
}

// 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
}
 
int main() {
    pc.baud(115200);
    pc.printf("Program BIOROBOTICS startup\r\n");
    
    // Define Controller Values
    Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
    Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
    
    Motor2.SetOutputLimits(-0.15f,0.15f);
    Motor2.SetOutputLimits(-0.5f,0.5f);
    
    Motor1.SetPID(100.0f,0.0f,0.001f);
    Motor2.SetPID(100.0f,0.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(&LoopFunctionJacTransposed,Interval);
    GetRefTicker.attach(&DeterminePosRef,0.5f);
    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");
        break;
        case 25: //1
            pc.printf("2\n\r");
            break;
        case 13: //1                
            pc.printf("3\n\r");
            break;
        case 12: //1
            pc.printf("4\n\r");
            break;
        case 24: //1
            pc.printf("5\n\r");
            ControlTicker.detach();
            Motor1.Stop();
            Motor2.Stop();
            PickUp();
            //ControlTicker.attach(&ControlLoop, Interval);
            break;
        case 94: //1
            pc.printf("6\n\r");
            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(&ControlLoop, Interval);
            break;
        case 90: //1
            pc.printf("9\n\r");
            break;
        case 70: //1
            pc.printf("Boven\n\r");
            //PosRef2=PosRef2-0.1f;
            /*
            GYset = GYset + 1;
            */
            //pc.printf("%f\n\r", PosRef2);
            break;
        case 21: //1
            pc.printf("Onder\n\r");
            //PosRef2=PosRef2+0.1f;  
            /*
            GYset = GYset - 1;
            */
            //pc.printf("%f\n\r", PosRef2);
            break;
        case 68: //1
            pc.printf("Links\n\r");
            //PosRef1=PosRef1+0.1f;
            /*
            GXset = GXset + 1;
            */
            //pc.printf("%f\n\r", PosRef1);
            break;
        case 67: //1
            pc.printf("Rechts\n\r");
            //PosRef1=PosRef1-0.1f;
            /*
            GXset = GXset - 1;
            */
            //pc.printf("%f\n\r", PosRef1);
            break;
        case 64: //1
            pc.printf("OK\n\r");
            //ControlTicker.detach();
            //MotorThrottle1=0.0f;
            //MotorThrottle2=0.0f;
            //HomingLoop();
            //ControlTicker.attach(&ControlLoop, Interval);
            
            break;
        default:
            break;
    }
}