main.cpp

00001 #include "mbed.h"
00002 #include "stm32f103c8t6.h"
00003 const int ID = 3;
00004 
00005 CANMessage msg;
00006 CAN Reseive (PA_11, PA_12);
00007 InterruptIn A(PB_0);//A0
00008 DigitalIn B(PB_1);//A1
00009 Ticker pid;
00010 
00011 /*-----モーター---*/
00012 //z軸
00013 PwmOut pwmZ(PB_3);//D3
00014 DigitalOut z1(PB_4);//D4
00015 DigitalOut z2(PA_15);//D5
00016 //y軸
00017 PwmOut pwmY(PA_8);//D3
00018 DigitalOut y1(PB_14);//D4
00019 DigitalOut y2(PB_15);//D5
00020 
00021 
00022 DigitalOut led(PC_13);
00023 
00024 DigitalIn switch1(PA_1);//A2
00025 DigitalIn switch2(PA_2);//A2
00026 DigitalIn switch3(PA_3);//A2
00027 
00028 /*-----PID変数----*/
00029 const float Kp = 0.0006;
00030 const float Ki = 0.001;
00031 const float Kd = 0.0;
00032 float p,i,d;
00033 float prep = 0.0;
00034 float goal = 0.0;//目標回転数
00035 
00036 
00037 long Count = 0;
00038 float val = 0.0;
00039 const float interval = 0.002;
00040 const float basisPowY = 0.25;
00041 const float basisPowZ = 0.25;
00042 
00043 int ymode = 0;
00044 
00045 bool finishFlagY = false;
00046 bool finishFlagZ = false;
00047 
00048 /*----受信割り込み---*/
00049 void inter(){
00050     Reseive.read(msg);
00051     if(msg.id == ID){
00052         finishFlagY = false;
00053         finishFlagZ = false;
00054         int z = int(msg.data[0]);
00055         int y = int(msg.data[1]);
00056         goal = float(-1*z);
00057         ymode = y;
00058     }else if(msg.id == (ID+4)){//フラッグチェック用
00059         char p;
00060         if((finishFlagY == 1) && (finishFlagZ == 1)){
00061             p = 1;
00062         }else{
00063             p = 0;
00064         }
00065         Reseive.write( CANMessage( ID+14, &p, 1) );
00066         //wait_ms(5);
00067     }      
00068 }
00069 
00070 void upZ(){
00071     if(ID == 0 || ID == 3){
00072         z1 = 0;
00073         z2 = 1;
00074     }else{
00075         z1 = 1;
00076         z2 = 0;
00077     }
00078 }
00079 
00080 void downZ(){
00081     if(ID == 0 || ID == 3){
00082         z1 = 1;
00083         z2 = 0;
00084     }else{
00085         z1 = 0;
00086         z2 = 1;
00087     }
00088 }
00089 
00090 void forwardY(){
00091     if(ID == 0 || ID == 2){
00092         y1 = 1;
00093         y2 = 0;
00094     }else{
00095         y1 = 0;
00096         y2 = 1;
00097     }
00098 }
00099 void backY(){
00100     if(ID == 0 || ID == 2){
00101         y1 = 0;
00102         y2 = 1;
00103     }else{
00104         y1 = 1;
00105         y2 = 0;
00106     }
00107 }
00108 void stopZ(){
00109     z1 = 1;
00110     z2 = 1;
00111 }
00112 void stopY(){
00113     y1 = 1;
00114     y2 = 1;
00115 }
00116 void Arise(){
00117     if(int(B) == 0){
00118         Count++;
00119     }else{
00120         Count--;
00121     }
00122         
00123 }
00124 
00125 void Afall(){
00126     if(int(B) == 0){
00127         Count--;
00128     }else{
00129         Count++;
00130     }
00131         
00132 }
00133 
00134 void Update(){
00135     long now = Count;
00136     if(ID == 1 || ID == 2){
00137         now = now * -1;
00138     }
00139     p = goal - float(now);//比例
00140     i += p * interval;//積分
00141     d = (p - prep)/interval;//微分
00142     prep = p;
00143     float output = (Kp*p + Ki*i + Kd*d);
00144     if(output > 0){
00145         downZ();
00146         if(switch1.read() == 0){
00147             stopZ();
00148             output = 0;
00149             //Count = 0;
00150         }
00151     }else if(output < 0){
00152         upZ();
00153         led = 0;
00154     }else{
00155         stopZ();
00156         output = 0;
00157         finishFlagZ = true;
00158         
00159     }
00160     if(output > (1 - basisPowZ)){
00161         output = 1 - basisPowZ;
00162     }else if(output < (basisPowZ - 1)){
00163         output = basisPowZ -1;
00164     }
00165     if(p <= 3 && p >= -3){
00166         stopZ();
00167         output = 0;
00168         finishFlagZ = true;
00169     }else{
00170         //finishFlagZ = false;
00171     }
00172     output = abs(output);
00173     pwmZ = basisPowZ + output;
00174 }
00175 
00176 int main(){
00177     led = 1;
00178     Reseive.frequency(100000);
00179     Reseive.attach( &inter);
00180     switch1.mode(PullDown);
00181     switch2.mode(PullDown);
00182     switch3.mode(PullDown);
00183     pwmZ.period_us(50);
00184     pwmY.period_us(50);
00185     
00186     //初期位置
00187     pwmZ = basisPowZ;
00188     downZ();
00189     pwmY = basisPowY;
00190     stopY();
00191     while(1){
00192         if(switch1.read() == 0){
00193             stopZ();
00194             break;
00195         }
00196     }
00197     pid.attach(&Update,interval);
00198     A.mode(PullUp);
00199     B.mode(PullUp);
00200     A.rise(&Arise);
00201     A.fall(&Afall);
00202     //led = 0;
00203     while (true) {
00204         if(finishFlagY == false){
00205             if(ymode == 1){
00206                 if(switch2.read() == 0){
00207                     stopY();
00208                     finishFlagY = true;
00209                 }else{//前へ
00210                     forwardY();
00211                     //finishFlagY = false;
00212                 }
00213             }else{
00214                 if(switch3.read() == 0){
00215                     stopY();
00216                     finishFlagY = true;
00217                 }else{//後ろへ
00218                     backY();
00219                     //finishFlagY = false;
00220                 }
00221             }
00222         }else{
00223             stopY();
00224         }
00225         if(switch1.read() == 0){
00226             Count = 0;
00227         }
00228     }
00229 }