atsushi takata
aa
Diff: main.cpp
- Revision:
- 0:5aefd5111bda
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