File content as of revision 0:e63996fd7d3e:

/*
this program is to test yaw controlling plane.
*/

#include "mbed.h"
#include "AngularDataAcquizition.h"
//#define CascadedPID
#include "PIDTheta.h"
#include "PID_Alt.h"
#include "PID_Yaw.h"

#define Listen_time_ms 20
#define ILowerRPM -3.14
#define IUpperRPM 3.14
#define OLowerPWM 0.001
#define OUpperPWM 0.002
#define OLowerPWM_yaw 0.001
#define OUpperPWM_yaw 0.0016


#define ILowerIR 27
#define IUpperIR 125
#define OLowerPower 0.00115
#define OUpperPower 0.0018

#define newILowerIR 27
#define newIUpperIR 125
#define newOLowerPower 0.00135
#define newOUpperPower 0.00145


#define KPgain 0.0000755/*0.000055   /*0.0000759*/
#define KDgain 0.0009/*0.0007   /*0.001106*/
#define KDDgain 0.0014/*0.0014*/

#define Yaw_KP 0.0000755/*00025*/
#define Yaw_KI 0.000
#define Yaw_KD 0.0009/*001*/
#define Yaw_KDD 0.0014/*0011*/


#define Pitch_KP KPgain
#define Pitch_KI 0.000
#define Pitch_KD KDgain

#define Roll_KP KPgain
#define Roll_KI 0.000
#define Roll_KD KDgain

#define IR_KP 00.00000005
#define IR_KI 0.00
#define IR_KD 0.000001
#define IR_KDD 0.000000

#define MeanNum 5
#define learningRate 0.00000001

#define _sig_A_Kp 50
#define _sig_A_Kd 100
Timer t;
//Serial pc(USBTX, USBRX);
Serial pc(p13, p14);
DigitalOut myled(LED1);
LocalFileSystem local("local");                    //////setting the destination of the file that will be saved

AngularDataAcquizition Angle(p9, p10);
PID_Yaw Yaw_Plane(Yaw_KP,Yaw_KI,Yaw_KD,Yaw_KDD);
PIDTheta Pitch_Plan(p21,p23,Listen_time_ms,Pitch_KP,Pitch_KI,Pitch_KD,KDDgain);
PIDTheta Roll_Plan(p22,p24,Listen_time_ms,Roll_KP,Roll_KI,Roll_KD,KDDgain);
PID_Alt PID_IR(MeanNum,IR_KP,IR_KI,IR_KD,IR_KDD,p20);

//prototypes
void PWM_change();
void PWM_change2();
void alt_update_Val();
void sendstatus();
float val,val2=0,val_test=0,x_plane,y_plane;
char Calibrated;
char flag=0;
char selected_plane=3;
float KD_NEW,KP_NEW;
float AngleRoll,AnglePitch,AngleOffset[2],AngleYaw,yawOffset,y_old;
Ticker timer,timer2;
float acc_PitchPlane,acc_RollPlane,Pitchacc_Req,Rollacc_Req,Req_Pitch,Req_Roll;

int main() {
    KD_NEW=Pitch_KD;
    KP_NEW=Pitch_KP;
    //pc.baud(9600);
    pc.baud(115200);
    //PID_IR
    PID_IR.Set_IN_Limits(ILowerIR,IUpperIR);
    PID_IR.Set_OUT_Limits(OLowerPower,OUpperPower);

    AngleOffset[0]=0;
    AngleOffset[1]=0;
    yawOffset=0;

    //Yaw_Plan
    Yaw_Plane.Set_IN_Limits(ILowerRPM,IUpperRPM);
    Yaw_Plane.Set_OUT_Limits(OLowerPWM_yaw,OUpperPWM_yaw);
    //Pitch_Plan
    Pitch_Plan.Set_IN_Limits(ILowerRPM,IUpperRPM);
    Pitch_Plan.Set_OUT_Limits(OLowerPWM,OUpperPWM);
    //Roll_Plan
    Roll_Plan.Set_IN_Limits(ILowerRPM,IUpperRPM);
    Roll_Plan.Set_OUT_Limits(OLowerPWM,OUpperPWM);
    //Angle.BeginInterrupt(0.039);
    acc_PitchPlane=0;
    acc_RollPlane=0;
    Pitchacc_Req=0;
    Rollacc_Req=0;
    Req_Pitch=0;
    Req_Roll=0;
    val=0.00115;
    Calibrated=0;
    val2=0;
  while(1){  
    while(!pc.readable());
        pc.getc();
   // timer.attach(alt_update_Val, 0.15);
    timer2.attach(sendstatus,0.25);
    
    t.reset();
    t.start();
        FILE *fp=fopen("/local/out.xls","a");

    while(1) {
        if(Angle.dmpReady)
            {
            PWM_change();
            Angle.callMeanFilteredReadings();
             //PWM_change2();
            //val_test=PID_IR.findNominalVal(val2,cos(AnglePitch),cos(AngleRoll),Calibrated);
            
            AngleYaw=Angle.Meanypr[0]-yawOffset;
            AnglePitch=(Angle.Meanypr[1]-AngleOffset[0]);
            AngleRoll=(Angle.Meanypr[2]-AngleOffset[1]);
           if(abs(AngleYaw-y_old)<0.1*M_PI/180)yawOffset=yawOffset+(AngleYaw-y_old);
                y_old=AngleYaw;
            /*
            acc_PitchPlane=sin(AngleRoll);
            acc_RollPlane=sin(AnglePitch);
            Req_Pitch=asin(Pitchacc_Req-acc_PitchPlane);
            Req_Roll=asin(Rollacc_Req-acc_RollPlane);
            if(Req_Pitch>5)Req_Pitch=10;
            if(Req_Pitch<-5)Req_Pitch=-10;
            if(Req_Roll>5)Req_Roll=10;
            if(Req_Roll<-5)Req_Roll=-10;*/
            
            Yaw_Plane.PWM_cal(0,AngleYaw,val,&x_plane,&y_plane);
            
            if(selected_plane==1||selected_plane==3){
                Pitch_Plan.PWM_cal(0*3.14/180/*0*/,-1*AnglePitch,x_plane/*val*/);    
                }
            if(selected_plane==2||selected_plane==3){
                Roll_Plan.PWM_cal(0,1*AngleRoll,y_plane/*val*/);
                }
            //fprintf(fp,"%f \t %f \t %f \t\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,AngleYaw*180/3.14);

            //pc.printf("A1:%f A2:%f OP:%f OP2:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val_test);
            //pc.printf("A1:%f A2:%f OP:%f RAlt:%f OP_test:%f alt:%f t:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val2,val_test,PID_IR.MeanIR(3),t.read());                  
           // fprintf(fp,"%f \t %f \t %f \t %f \t %f \t",AngleRoll,Req_Roll,AnglePitch,Req_Pitch,t.read());

/*            fprintf(fp,"%f \t %f \t %f \t %f \t %f \t %f \t %f \t %f \t %f \t %f \t %f \t %f\r\n",(AnglePitch)*180/3.14,Pitch_Plan.readErrorVal(),Pitch_Plan.readErrorDVal(),Pitch_Plan.readGradientFunction(),Pitch_Plan.readSigmoindFunVal()
                                                                                            ,(AngleRoll)*180/3.14,Roll_Plan.readErrorVal(),Roll_Plan.readErrorDVal(),Roll_Plan.readGradientFunction(),Roll_Plan.readSigmoindFunVal()
                                                                                            ,val,t.read());
*/       }
            if(flag){
                flag=0;
                Pitch_Plan.PWM_cal(0,0,0.001);
                Roll_Plan.PWM_cal(0,0,0.001);
                Pitch_Plan.PWM_cal(0,0,0.001);
                Roll_Plan.PWM_cal(0,0,0.001);
                //timer.detach();
                val2=0;
                break;}
        }
        fclose(fp);

    }
}
void PWM_change()
{
    if(pc.readable()) {
        switch (pc.getc()) {
            case 'u':
                val += 0.00005;
                val2 += 10;
                break;
            case 'd':
                val -= 0.00005;
                val2 -= 5;
                break;
            case 'w':
                val += 0.00001;
                break;
            case 'x':
                val -= 0.00001;
                break;
            case 'e':
                val+= 0.000005;
                break;
            case 'f':
                val-=0.000005;
                break;
            case '*':
                val = 0.002;
                break;
            case 'm':
            val =0.001;
            break;
            case 's':
                val = 0.001;
                flag=1;
                break;
            case'1':
               KD_NEW+=0.1;
        //       Pitch_Plan.ChangeKDval(KD_NEW);
        //       Roll_Plan.ChangeKDval(KD_NEW); 
               break;
            case'2':
             KD_NEW-=0.1;
        //     Pitch_Plan.ChangeKDval(KD_NEW);
        //     Roll_Plan.ChangeKDval(KD_NEW); 
             break;
            case'4':
               KP_NEW+=0.001;
        //      Pitch_Plan.ChangeKDval(KD_NEW);
        //       Roll_Plan.ChangeKDval(KD_NEW); 
               break;
            case'5':
                KP_NEW-=0.001;
        //        Pitch_Plan.ChangeKPval(KD_NEW);
        //        Roll_Plan.ChangeKPval(KD_NEW); 
             break;
            case 'c':
                AngleOffset[0]=Angle.Meanypr[1];
                AngleOffset[1]=Angle.Meanypr[2];
                yawOffset=Angle.Meanypr[0];

                Calibrated=1;

            break;
            /*case '1':
            case '2':
            case '4':
            case '5':
          */case '7':
                selected_plane=1;
                val = 0.001;
            break;
            case '8':
                selected_plane=2;
                val = 0.001;
            break;
            case '9':
                selected_plane=3;
                val = 0.001;
            break;
        
            case 'l':
                PID_IR.Set_IN_Limits(newILowerIR,newIUpperIR);
                PID_IR.Set_OUT_Limits(newOLowerPower,newOUpperPower);
            break;
            case 'p':
                PID_IR.Set_IN_Limits(ILowerIR,IUpperIR);
                PID_IR.Set_OUT_Limits(OLowerPower,OUpperPower);
            break;

        }
      //  pc.printf("char:%c",pc.getc());

    }

}


void PWM_change2(){//distance in cm
    if(pc.readable()) {
        switch (pc.getc()) {
            case 'u':
            val2 += 5;
            break;
            case 'd':
            val2-=5;
            break;
            case 's':
            val2=0;
            flag=1;
            break;
            case 'c':
                AngleOffset[0]=Angle.Meanypr[1];
                AngleOffset[1]=Angle.Meanypr[2];
                yawOffset=Angle.Meanypr[0];
                Calibrated=1;
            break;
            case 'm':
                PID_IR.Set_IN_Limits(newILowerIR,newIUpperIR);
                PID_IR.Set_OUT_Limits(newOLowerPower,newOUpperPower);
            break;
            case 'p':
                PID_IR.Set_IN_Limits(ILowerIR,IUpperIR);
                PID_IR.Set_OUT_Limits(OLowerPower,OUpperPower);
            break;

            }
        }
    }
void alt_update_Val(){
    val_test=PID_IR.findNominalVal(val2,cos(AnglePitch),cos(AngleRoll),Calibrated);
    //val=PID_IR.findNominalVal(val2,cos(AnglePitch),cos(AngleRoll),Calibrated);
    
    //pc.printf("here\r\n");
           
    }
void sendstatus(){
            //pc.printf("A1:%f A2:%f OP:%f OP2:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val_test);
            //pc.printf("A1:%f A2:%f OP:%f RAlt:%f OP_test:%f alt:%f t:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val2,val_test,PID_IR.MeanIR(3),t.read());                  
            pc.printf("A1:%f A2:%f A3:%f OP:%f RA:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,AngleYaw*180/3.14,val,val2);
    }