File content as of revision 0:5aefd5111bda:

#include "mbed.h"

PwmOut V1(D2);
PwmOut V2(D3);
PwmOut V3(D4);
PwmOut V4(D5);
PwmOut V5(D6);
PwmOut V6(D7);
AnalogIn J1(A1);
AnalogIn J2(A2);
AnalogIn J3(A0);

DigitalOut Led(LED1);
Serial pc(D1, D0);
Ticker cnt;

bool CntFlag=false;
bool PiFlag=false;
float Duty[7]={0.0f,0.5f,0.0f,0.3f,0.0f,0.0f,0.0f};
float j[4]={0.0f,0.0f,0.0f,0.0f};
float jref[4]={0.0f,0.45f,0.8f,0.7f};
float e[4]={0.0f,0.0f,0.0f,0.0f};
float eSum[4]={0.0f,0.0f,0.0f,0.0f};

float Kp[4]={0.0f,0.1f,4.0f,1.0f};
float Ki[4]={0.0f,1.0f,0.1f,0.01f};


void DutySet(void){
  

  for(int i=0;i<6;i++){
      if(Duty[i+1]>1.0f){
          Duty[i+1]=1.0f;
      }
      if(Duty[i+1]<0.0f){
          Duty[i+1]=0.0f;
      }
      pc.printf("%1.3f,",Duty[i+1]);
  }
  V1=Duty[1];
  V2=Duty[2];
  V3=Duty[3];
  V4=Duty[4];
  V5=Duty[5];
  V6=Duty[6];  
}

void ControlFlag(void){
    CntFlag=true;
}

int main() {
    pc.baud(9600);
    int Period=10;
    char order=0;
    V1.period_ms(Period);
    V2.period_ms(Period);
    V3.period_ms(Period);
    V4.period_ms(Period);
    V5.period_ms(Period);
    V6.period_ms(Period);
    
    float jrefLimit[4][2];
    jrefLimit[1][0]=0.340;
    jrefLimit[1][1]=0.424;
    jrefLimit[2][0]=0.673;
    jrefLimit[2][1]=0.765;
    jrefLimit[3][0]=0.653;
    jrefLimit[3][1]=0.879;
    
    float pressure[4];
    
    cnt.attach(&ControlFlag,0.001f);
    
    while(1) {
        if(CntFlag){
            j[1]=J1.read();
            j[2]=J2.read();
            j[3]=J3.read();
            pc.printf("J:%1.3f,%1.3f,%1.3f",j[1],j[2],j[3]);
            pc.printf("Jref:%1.3f,%1.3f,%1.3f",jref[1],jref[2],jref[3]);
            
            if(PiFlag){
                pressure[1]=0.4f;
                pressure[2]=0.0f;
                pressure[3]=0.2f;
                
                for(int i=1;i<4;i++){
                    e[i]=jref[i]-j[i];
                    eSum[i]+=e[i];
                    if(eSum[i]*Ki[i]>1.0f){
                        eSum[i]=1.0f/Ki[i];
                    }
                    if(eSum[i]*Ki[i]<-1.0f){
                        eSum[i]=-1.0f/Ki[i];
                    }
                    pressure[i]+=Kp[i]*e[i]+Ki[i]*eSum[i];
                    ;
                    Duty[2*i-1]=0.5f+pressure[i]/2.0f;
                    Duty[2*i]  =0.5f-pressure[i]/2.0f;
                    if((i==1)||(i==3)){
                        Duty[2*i-1]=0.0f;
                        Duty[2*i]  =0.0f;
                    
                    }
                                 
                }   
                
            }
            
            pc.printf("Duty:");
            DutySet();
            pc.printf("PiFlag:%i",PiFlag);
            if(pc.readable()){
                order = pc.getc();
               switch(order){
                    case 'q':
                        Duty[1]+=0.01f;
                        break;
                    case 'a':
                        Duty[1]-=0.01f;
                        break;
                    case 'w':
                        Duty[2]=Duty[2]+0.01f;
                        break;
                    case 's':
                        Duty[2]-=0.01f;
                        break;
                    case 'e':
                        Duty[3]+=0.01f;
                        break;
                    case 'd':
                        Duty[3]-=0.01f;
                        break;
                    case 'r':
                        Duty[4]+=0.01f;
                        break;
                    case 'f':
                        Duty[4]-=0.01f;
                        break;
                    case 't':
                        Duty[5]+=0.01f;
                        break;
                    case 'g':
                        Duty[5]-=0.01f;
                        break;
                    case 'y':
                        Duty[6]+=0.01f;
                        break;
                    case 'h':
                        Duty[6]-=0.01f;
                        break;
                    case 'u':
                        jref[1]+=0.01f;
                        break;
                    case 'j':
                        jref[1]-=0.01f;
                        break;
                    case 'i':
                        jref[2]+=0.01f;
                        break;
                    case 'k':
                        pc.printf(",k,,");
                        jref[2]-=0.01f;
                        break;
                    case 'o':
                        jref[3]+=0.01f;
                        break;
                    case 'l':
                        jref[3]-=0.01f;
                        break;
                    case 'z':
                        PiFlag=true;
                        break;
                    case 'x':
                        PiFlag=false;
                        break;
                    
                    default:
                        break;
                }
                for(int i=1;i<4;i++){
                        if(jref[i]<jrefLimit[i][0]){
                            jref[i]=jrefLimit[i][0];
                        }
                        if(jref[i]>jrefLimit[i][1]){
                            jref[i]=jrefLimit[i][1];
                        }                           
                }
                pc.putc(order);    
            }
        CntFlag=false;   
        pc.printf("\r\n");
        }//if(CntFlag)
        
    }//while(1)
}//main