新部内対抗A班 / Mbed 2 deprecated circle_war_ver_A_NUCLEO__

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of circle_war_ver_A_NUCLEO_ by 新部内対抗A班

function.h

Committer:
baba2357
Date:
2016-04-05
Revision:
14:3403ce49a37a
Parent:
12:2450012ce888

File content as of revision 14:3403ce49a37a:

// pid,valve,interrupt,setupを定義
//上下機構&アーム
InterruptIn a(PC_8);
InterruptIn b(PC_6);
PwmOut bole_f(PB_4);
PwmOut bole_b(PB_5);
DigitalOut valve1(PB_7);
//PID
void pid(float t,int count,double rate,bool c){
    target=t;
    dist_n=2*PI*radius*rate*count/4096.0f;
    
    dt=count_time.read();
    count_time.reset();
    count_time.start();
    
    devi=t-dist_n;    
    diff=(dist_n-dist_p)/dt;    
    inte+=(dist_n-dist_p)*dt;
    
    dist_p=dist_n;
    
    
    pwm=Kp_a*devi+Kd_a*diff+Ki_a*inte;
    pwm/=ratio;
    
    if(pwm>MAX){
        pwm=MAX;
    }else if(pwm<-MAX){
        pwm=-MAX;
    }
    
    if(!c){     //幹
        if(pwm>=0){
            bole_f=pwm;
            bole_b=0;
        }else{
            bole_f=0;
            bole_b=-pwm;
        }
    }else{      //桜
/*        if(pwm>=0){
            bloss_f=pwm;
            bloss_b=0;
        }else{
            bloss_f=0;
            bloss_b=-pwm;
        }    
    }
*/
}

}
//valve
void valve(bool c){
    valve1=c;
//    if(c){
        wait(1);
//        }
}


//interrupt-bole
void risea(){
    pa=1;
    if(pb==0) {
        count++;
    }else {
        count--;
    }
}
void falla(){
    pa=0;
    if(pb==0) {
        count--;
    }else {
        count++;
    }
}
void riseb(){
    pb=1;
    if(pa==0) {
        count--;
    }else {
        count++;
    }
}
void fallb(){
    pb=0;
    if(pa==0) {
        count++;
    }else {
        count--;
    }
}


//interrupt-blos
/*
void risea2()
{
    pa2=1;
    if(pb2==0) {
        count2++;
    }

    else {
        count2--;
    }
}
void falla2()
{
    pa2=0;
    if(pb2==0) {
        count2--;
    }

    else {
        count2++;
    }
}
void riseb2()
{
    pb2=1;
    if(pa2==0) {
        count2--;
    }

    else {
        count2++;
    }
}
void fallb2()
{
    pb2=0;
    if(pa2==0) {
        count2++;
    }

    else {
        count2--;
    }
}
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////





void setup(){
    
    //interrupt
    a.rise(&risea);                       //A interrupt
    a.fall(&falla);                       //A interrupt
    b.rise(&riseb);                       //B interrupt
    b.fall(&fallb);                       //B interrupt

/*   a2.rise(&risea2);                     //A2 interrupt
    a2.fall(&falla2);                     //A2 interrupt
    b2.rise(&riseb2);                     //B2 interrupt
    b2.fall(&fallb2);                     //B2 interrupt
*/    
    count=0;
/*    count2=0;
*/    
    
    //set-motor.period
    bole_f.period_us(50);
    bole_b.period_us(50);
/*    bloss_f.period_us(50);
    bloss_b.period_us(50);
    catch1_f.period_us(50);
    catch1_b.period_us(50);
    catch2_f.period_us(50);
    catch2_b.period_us(50);
    
    //touchsensor-pullup
    sw1.mode(PullUp);
    sw2.mode(PullUp);
*/    
}