atsushi takata / Mbed 2 deprecated 180831AirArmAnalogValvePI

aa

Dependencies:   mbed

main.cpp@0:5aefd5111bda, 2018-09-03 (annotated)

Committer:
tktats_892476
Date:
Mon Sep 03 05:37:40 2018 +0000
Revision:
0:5aefd5111bda
180903;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tktats_892476 0:5aefd5111bda 1 #include "mbed.h"
tktats_892476 0:5aefd5111bda 2
tktats_892476 0:5aefd5111bda 3 PwmOut V1(D2);
tktats_892476 0:5aefd5111bda 4 PwmOut V2(D3);
tktats_892476 0:5aefd5111bda 5 PwmOut V3(D4);
tktats_892476 0:5aefd5111bda 6 PwmOut V4(D5);
tktats_892476 0:5aefd5111bda 7 PwmOut V5(D6);
tktats_892476 0:5aefd5111bda 8 PwmOut V6(D7);
tktats_892476 0:5aefd5111bda 9 AnalogIn J1(A1);
tktats_892476 0:5aefd5111bda 10 AnalogIn J2(A2);
tktats_892476 0:5aefd5111bda 11 AnalogIn J3(A0);
tktats_892476 0:5aefd5111bda 12
tktats_892476 0:5aefd5111bda 13 DigitalOut Led(LED1);
tktats_892476 0:5aefd5111bda 14 Serial pc(D1, D0);
tktats_892476 0:5aefd5111bda 15 Ticker cnt;
tktats_892476 0:5aefd5111bda 16
tktats_892476 0:5aefd5111bda 17 bool CntFlag=false;
tktats_892476 0:5aefd5111bda 18 bool PiFlag=false;
tktats_892476 0:5aefd5111bda 19 float Duty[7]={0.0f,0.5f,0.0f,0.3f,0.0f,0.0f,0.0f};
tktats_892476 0:5aefd5111bda 20 float j[4]={0.0f,0.0f,0.0f,0.0f};
tktats_892476 0:5aefd5111bda 21 float jref[4]={0.0f,0.45f,0.8f,0.7f};
tktats_892476 0:5aefd5111bda 22 float e[4]={0.0f,0.0f,0.0f,0.0f};
tktats_892476 0:5aefd5111bda 23 float eSum[4]={0.0f,0.0f,0.0f,0.0f};
tktats_892476 0:5aefd5111bda 24
tktats_892476 0:5aefd5111bda 25 float Kp[4]={0.0f,0.1f,4.0f,1.0f};
tktats_892476 0:5aefd5111bda 26 float Ki[4]={0.0f,1.0f,0.1f,0.01f};
tktats_892476 0:5aefd5111bda 27
tktats_892476 0:5aefd5111bda 28
tktats_892476 0:5aefd5111bda 29 void DutySet(void){
tktats_892476 0:5aefd5111bda 30
tktats_892476 0:5aefd5111bda 31
tktats_892476 0:5aefd5111bda 32 for(int i=0;i<6;i++){
tktats_892476 0:5aefd5111bda 33 if(Duty[i+1]>1.0f){
tktats_892476 0:5aefd5111bda 34 Duty[i+1]=1.0f;
tktats_892476 0:5aefd5111bda 35 }
tktats_892476 0:5aefd5111bda 36 if(Duty[i+1]<0.0f){
tktats_892476 0:5aefd5111bda 37 Duty[i+1]=0.0f;
tktats_892476 0:5aefd5111bda 38 }
tktats_892476 0:5aefd5111bda 39 pc.printf("%1.3f,",Duty[i+1]);
tktats_892476 0:5aefd5111bda 40 }
tktats_892476 0:5aefd5111bda 41 V1=Duty[1];
tktats_892476 0:5aefd5111bda 42 V2=Duty[2];
tktats_892476 0:5aefd5111bda 43 V3=Duty[3];
tktats_892476 0:5aefd5111bda 44 V4=Duty[4];
tktats_892476 0:5aefd5111bda 45 V5=Duty[5];
tktats_892476 0:5aefd5111bda 46 V6=Duty[6];
tktats_892476 0:5aefd5111bda 47 }
tktats_892476 0:5aefd5111bda 48
tktats_892476 0:5aefd5111bda 49 void ControlFlag(void){
tktats_892476 0:5aefd5111bda 50 CntFlag=true;
tktats_892476 0:5aefd5111bda 51 }
tktats_892476 0:5aefd5111bda 52
tktats_892476 0:5aefd5111bda 53 int main() {
tktats_892476 0:5aefd5111bda 54 pc.baud(9600);
tktats_892476 0:5aefd5111bda 55 int Period=10;
tktats_892476 0:5aefd5111bda 56 char order=0;
tktats_892476 0:5aefd5111bda 57 V1.period_ms(Period);
tktats_892476 0:5aefd5111bda 58 V2.period_ms(Period);
tktats_892476 0:5aefd5111bda 59 V3.period_ms(Period);
tktats_892476 0:5aefd5111bda 60 V4.period_ms(Period);
tktats_892476 0:5aefd5111bda 61 V5.period_ms(Period);
tktats_892476 0:5aefd5111bda 62 V6.period_ms(Period);
tktats_892476 0:5aefd5111bda 63
tktats_892476 0:5aefd5111bda 64 float jrefLimit[4][2];
tktats_892476 0:5aefd5111bda 65 jrefLimit[1][0]=0.340;
tktats_892476 0:5aefd5111bda 66 jrefLimit[1][1]=0.424;
tktats_892476 0:5aefd5111bda 67 jrefLimit[2][0]=0.673;
tktats_892476 0:5aefd5111bda 68 jrefLimit[2][1]=0.765;
tktats_892476 0:5aefd5111bda 69 jrefLimit[3][0]=0.653;
tktats_892476 0:5aefd5111bda 70 jrefLimit[3][1]=0.879;
tktats_892476 0:5aefd5111bda 71
tktats_892476 0:5aefd5111bda 72 float pressure[4];
tktats_892476 0:5aefd5111bda 73
tktats_892476 0:5aefd5111bda 74 cnt.attach(&ControlFlag,0.001f);
tktats_892476 0:5aefd5111bda 75
tktats_892476 0:5aefd5111bda 76 while(1) {
tktats_892476 0:5aefd5111bda 77 if(CntFlag){
tktats_892476 0:5aefd5111bda 78 j[1]=J1.read();
tktats_892476 0:5aefd5111bda 79 j[2]=J2.read();
tktats_892476 0:5aefd5111bda 80 j[3]=J3.read();
tktats_892476 0:5aefd5111bda 81 pc.printf("J:%1.3f,%1.3f,%1.3f",j[1],j[2],j[3]);
tktats_892476 0:5aefd5111bda 82 pc.printf("Jref:%1.3f,%1.3f,%1.3f",jref[1],jref[2],jref[3]);
tktats_892476 0:5aefd5111bda 83
tktats_892476 0:5aefd5111bda 84 if(PiFlag){
tktats_892476 0:5aefd5111bda 85 pressure[1]=0.4f;
tktats_892476 0:5aefd5111bda 86 pressure[2]=0.0f;
tktats_892476 0:5aefd5111bda 87 pressure[3]=0.2f;
tktats_892476 0:5aefd5111bda 88
tktats_892476 0:5aefd5111bda 89 for(int i=1;i<4;i++){
tktats_892476 0:5aefd5111bda 90 e[i]=jref[i]-j[i];
tktats_892476 0:5aefd5111bda 91 eSum[i]+=e[i];
tktats_892476 0:5aefd5111bda 92 if(eSum[i]*Ki[i]>1.0f){
tktats_892476 0:5aefd5111bda 93 eSum[i]=1.0f/Ki[i];
tktats_892476 0:5aefd5111bda 94 }
tktats_892476 0:5aefd5111bda 95 if(eSum[i]*Ki[i]<-1.0f){
tktats_892476 0:5aefd5111bda 96 eSum[i]=-1.0f/Ki[i];
tktats_892476 0:5aefd5111bda 97 }
tktats_892476 0:5aefd5111bda 98 pressure[i]+=Kp[i]*e[i]+Ki[i]*eSum[i];
tktats_892476 0:5aefd5111bda 99 ;
tktats_892476 0:5aefd5111bda 100 Duty[2*i-1]=0.5f+pressure[i]/2.0f;
tktats_892476 0:5aefd5111bda 101 Duty[2*i] =0.5f-pressure[i]/2.0f;
tktats_892476 0:5aefd5111bda 102 if((i==1)||(i==3)){
tktats_892476 0:5aefd5111bda 103 Duty[2*i-1]=0.0f;
tktats_892476 0:5aefd5111bda 104 Duty[2*i] =0.0f;
tktats_892476 0:5aefd5111bda 105
tktats_892476 0:5aefd5111bda 106 }
tktats_892476 0:5aefd5111bda 107
tktats_892476 0:5aefd5111bda 108 }
tktats_892476 0:5aefd5111bda 109
tktats_892476 0:5aefd5111bda 110 }
tktats_892476 0:5aefd5111bda 111
tktats_892476 0:5aefd5111bda 112 pc.printf("Duty:");
tktats_892476 0:5aefd5111bda 113 DutySet();
tktats_892476 0:5aefd5111bda 114 pc.printf("PiFlag:%i",PiFlag);
tktats_892476 0:5aefd5111bda 115 if(pc.readable()){
tktats_892476 0:5aefd5111bda 116 order = pc.getc();
tktats_892476 0:5aefd5111bda 117 switch(order){
tktats_892476 0:5aefd5111bda 118 case 'q':
tktats_892476 0:5aefd5111bda 119 Duty[1]+=0.01f;
tktats_892476 0:5aefd5111bda 120 break;
tktats_892476 0:5aefd5111bda 121 case 'a':
tktats_892476 0:5aefd5111bda 122 Duty[1]-=0.01f;
tktats_892476 0:5aefd5111bda 123 break;
tktats_892476 0:5aefd5111bda 124 case 'w':
tktats_892476 0:5aefd5111bda 125 Duty[2]=Duty[2]+0.01f;
tktats_892476 0:5aefd5111bda 126 break;
tktats_892476 0:5aefd5111bda 127 case 's':
tktats_892476 0:5aefd5111bda 128 Duty[2]-=0.01f;
tktats_892476 0:5aefd5111bda 129 break;
tktats_892476 0:5aefd5111bda 130 case 'e':
tktats_892476 0:5aefd5111bda 131 Duty[3]+=0.01f;
tktats_892476 0:5aefd5111bda 132 break;
tktats_892476 0:5aefd5111bda 133 case 'd':
tktats_892476 0:5aefd5111bda 134 Duty[3]-=0.01f;
tktats_892476 0:5aefd5111bda 135 break;
tktats_892476 0:5aefd5111bda 136 case 'r':
tktats_892476 0:5aefd5111bda 137 Duty[4]+=0.01f;
tktats_892476 0:5aefd5111bda 138 break;
tktats_892476 0:5aefd5111bda 139 case 'f':
tktats_892476 0:5aefd5111bda 140 Duty[4]-=0.01f;
tktats_892476 0:5aefd5111bda 141 break;
tktats_892476 0:5aefd5111bda 142 case 't':
tktats_892476 0:5aefd5111bda 143 Duty[5]+=0.01f;
tktats_892476 0:5aefd5111bda 144 break;
tktats_892476 0:5aefd5111bda 145 case 'g':
tktats_892476 0:5aefd5111bda 146 Duty[5]-=0.01f;
tktats_892476 0:5aefd5111bda 147 break;
tktats_892476 0:5aefd5111bda 148 case 'y':
tktats_892476 0:5aefd5111bda 149 Duty[6]+=0.01f;
tktats_892476 0:5aefd5111bda 150 break;
tktats_892476 0:5aefd5111bda 151 case 'h':
tktats_892476 0:5aefd5111bda 152 Duty[6]-=0.01f;
tktats_892476 0:5aefd5111bda 153 break;
tktats_892476 0:5aefd5111bda 154 case 'u':
tktats_892476 0:5aefd5111bda 155 jref[1]+=0.01f;
tktats_892476 0:5aefd5111bda 156 break;
tktats_892476 0:5aefd5111bda 157 case 'j':
tktats_892476 0:5aefd5111bda 158 jref[1]-=0.01f;
tktats_892476 0:5aefd5111bda 159 break;
tktats_892476 0:5aefd5111bda 160 case 'i':
tktats_892476 0:5aefd5111bda 161 jref[2]+=0.01f;
tktats_892476 0:5aefd5111bda 162 break;
tktats_892476 0:5aefd5111bda 163 case 'k':
tktats_892476 0:5aefd5111bda 164 pc.printf(",k,,");
tktats_892476 0:5aefd5111bda 165 jref[2]-=0.01f;
tktats_892476 0:5aefd5111bda 166 break;
tktats_892476 0:5aefd5111bda 167 case 'o':
tktats_892476 0:5aefd5111bda 168 jref[3]+=0.01f;
tktats_892476 0:5aefd5111bda 169 break;
tktats_892476 0:5aefd5111bda 170 case 'l':
tktats_892476 0:5aefd5111bda 171 jref[3]-=0.01f;
tktats_892476 0:5aefd5111bda 172 break;
tktats_892476 0:5aefd5111bda 173 case 'z':
tktats_892476 0:5aefd5111bda 174 PiFlag=true;
tktats_892476 0:5aefd5111bda 175 break;
tktats_892476 0:5aefd5111bda 176 case 'x':
tktats_892476 0:5aefd5111bda 177 PiFlag=false;
tktats_892476 0:5aefd5111bda 178 break;
tktats_892476 0:5aefd5111bda 179
tktats_892476 0:5aefd5111bda 180 default:
tktats_892476 0:5aefd5111bda 181 break;
tktats_892476 0:5aefd5111bda 182 }
tktats_892476 0:5aefd5111bda 183 for(int i=1;i<4;i++){
tktats_892476 0:5aefd5111bda 184 if(jref[i]<jrefLimit[i][0]){
tktats_892476 0:5aefd5111bda 185 jref[i]=jrefLimit[i][0];
tktats_892476 0:5aefd5111bda 186 }
tktats_892476 0:5aefd5111bda 187 if(jref[i]>jrefLimit[i][1]){
tktats_892476 0:5aefd5111bda 188 jref[i]=jrefLimit[i][1];
tktats_892476 0:5aefd5111bda 189 }
tktats_892476 0:5aefd5111bda 190 }
tktats_892476 0:5aefd5111bda 191 pc.putc(order);
tktats_892476 0:5aefd5111bda 192 }
tktats_892476 0:5aefd5111bda 193 CntFlag=false;
tktats_892476 0:5aefd5111bda 194 pc.printf("\r\n");
tktats_892476 0:5aefd5111bda 195 }//if(CntFlag)
tktats_892476 0:5aefd5111bda 196
tktats_892476 0:5aefd5111bda 197 }//while(1)
tktats_892476 0:5aefd5111bda 198 }//main