Revision 0:5aefd5111bda, committed 2018-09-03

Comitter:

tktats_892476

Date:

Mon Sep 03 05:37:40 2018 +0000

Commit message:

180903;

Changed in this revision

diff -r 000000000000 -r 5aefd5111bda main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 03 05:37:40 2018 +0000
@@ -0,0 +1,198 @@
+#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

diff -r 000000000000 -r 5aefd5111bda mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Sep 03 05:37:40 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/a7c7b631e539
\ No newline at end of file