File content as of revision 0:0805c5a1b328:

#include <math.h>
#include "Utils.h"
#include "USBHost.h"
#include "hci.h"
#include "ps3.h"
#include "TestShell.h"
#include "User.h"
#include "caninit.h"
#include "mbed.h"
#ifndef M_PI
#define M_PI 3.14159265f
#endif
#ifdef common
#define ab 15
#define buf 14
#define ga 13
#define shdn 12
#define LSXcent 128
#define LSYcent 128
#define RSXcent 128
#define RSYcent 128

//Ticker ticker1;
Timer timer1;
CAN can1(p9, p10);
CANMessage msg;  
// pin settings
DigitalOut air1(p5);
DigitalOut air2(p6);
//SPI settings//
SPI spi(p11,p12,p13);
DigitalOut ss1(p20);
DigitalOut ss2(p19);
DigitalOut ss3(p18);
DigitalOut ss4(p17);
//PINsettings
DigitalOut ldac1(p21);
DigitalOut ldac2(p22);
DigitalOut ldac3(p23);
DigitalOut ldac4(p24);
// LED settings//
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
//
int serial_counter = 0;
int dac_data=0;
double debug_val = 0, debug_val2 = 0;
double real_val[4] = {};
double tar_val[4] = {};
double val[4] = {};
int send_val[4] = {};
double c_accel=5000;
char teisu=13;//23;
char can_send_data[8];

int con_x;
int con_y;
int con_theta;
double val_ratio, math1;
double sai_time[5]={}, sai_hensa[5]={}; 

long angle_ulong;
float raw_angle;
float angle;
float axis_angle = 0;
double sin2, cos2;
float kitai_angle = 0;
bool caninit_triger = 0;
int can_counter = 0;
double  avr_accel_x, avr_accel_y;


//
u8 RSX,RSY,LSX,LSY,BSU,BSL;
//コントローラ断線時対策用
u8 accel[5];
u8 accel_nowsub=0;
//////////
char cnt=0;


void UserLoopSetting(){
    spi.format(16,0);
    spi.frequency(1000000);//1M
    ss1=1;
    ss2=1;
    ss3=1;
    ss4=1;
    ldac1=1;
    ldac2=1;
    ldac3=1;
    ldac4=1;
}

void UserLoop(char n,const u8* data){
                //PS3conSTATE--->value
                u16 ButtonState;
                if(n==0){//有線Ps3USB.cpp
                    RSX = ((ps3report*)data)->RightStickX;
                    RSY = ((ps3report*)data)->RightStickY;
                    LSX = ((ps3report*)data)->LeftStickX;
                    LSY = ((ps3report*)data)->LeftStickY;
                    BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
                    BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
                    //ボタンの処理
                    ButtonState =  ((ps3report*)data)->ButtonState;
                }else {//無線TestShell.cpp
                    RSX = ((ps3report*)(data + 1))->RightStickX;
                    RSY = ((ps3report*)(data + 1))->RightStickY;
                    LSX = ((ps3report*)(data + 1))->LeftStickX;
                    LSY = ((ps3report*)(data + 1))->LeftStickY;
                    BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
                    BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
                    //ボタンの処理
                    ButtonState =  ((ps3report*)(data + 1))->ButtonState;
                }



                static long gyro_long;
                static short accel_long_x, accel_long_y;
                static double gyro_double, accel_double_x, accel_double_y;
                static double accel_x[AVR_ACCEL]={}, accel_y[AVR_ACCEL]={};
                float proportion, hensa;
                int i;
                double time1, diff;
                static bool timer1_init = 0;
                static bool lr_on, lr_off;
                static int lr_counter;
                static double raketto_time1, raketto_time2;
                static double accel_array[AVR_ACCEL]={};
                static bool down_state = 0;
                static bool up_state = 0;
                static bool start_state = 0, start_flag = 0;
                static double start_time = 0;
                
                
                for(int i = 0; i < 4; i++){
                    real_val[i] = val[i];
                }
                

                if(caninit_triger == 0){
                    can1.frequency(125000);
                    caninit_triger = 1;  
                }
                
                
                
                
                                             
                can1.read(msg); //CAN読み取り
                                     
                if(msg.id == 700){
                    static int angle_led;
                    angle_led ++;
                    if(angle_led > 2){
                        led2 = !led2;
                        angle_led = 0;
                    }
                    angle_ulong = ((unsigned long)msg.data[0] << 24) | ((unsigned long)msg.data[1] << 16) | ((unsigned long)msg.data[2] << 8) | ((unsigned long)msg.data[3]);       
                    raw_angle = -1.0*angle_ulong*180.0/(M_PI*100000.0);     
                    angle = raw_angle - axis_angle; // 軸からの角度を計算
                    if(angle < -180) angle += 360;
                    if(angle > 180) angle -= 360;
                    sin2 = sin((45-angle)*M_PI/180);
                    cos2 = cos((45-angle)*M_PI/180);
                }          
                if(msg.id == 700){
                    gyro_long = ((unsigned long)msg.data[0+4] << 24) | ((unsigned long)msg.data[1+4] << 16) | ((unsigned long)msg.data[2+4] << 8) | ((unsigned long)msg.data[3+4]);            
                    gyro_double = gyro_long/100000.0;
                }    
                if(msg.id == 710){
                    static int accel_led;
                    accel_led ++;
                    if(accel_led > 2){
                        led3 = !led3;
                        accel_led = 0;
                    }
                    accel_long_x = (msg.data[0] << 8) | (msg.data[1] );      
                    accel_long_y = ((unsigned long)msg.data[0+2] << 8) | ((unsigned long)msg.data[1+2]);      
                    accel_double_x = accel_long_x/100.0;
                    accel_double_y = accel_long_y/100.0;
                }
                
                float flt_buff = 0;
                flt_buff = real_val[0];
                TypeDivider( &flt_buff, can_send_data);
                
                //can1.write(CANMessage(300, &can_send_data[0], 4));
                
                
                
                         
                for(i=0;i<AVR_ACCEL-1;i++){
                    accel_x[i] = accel_x[i+1];
                }
                for(i=0;i<AVR_ACCEL-1;i++){
                    accel_y[i] = accel_y[i+1];
                }     
                accel_x[AVR_ACCEL-1] = accel_double_x;
                accel_y[AVR_ACCEL-1] = accel_double_y;
                
                for(i=0;i<AVR_ACCEL;i++){
                avr_accel_x += accel_x[i];
                }
                avr_accel_x = avr_accel_x/(AVR_ACCEL+1);
                for(i=0;i<AVR_ACCEL;i++){
                avr_accel_y += accel_y[i];
                }
                avr_accel_y = avr_accel_y/(AVR_ACCEL+1);
                 
                
                if(timer1_init == 1){
                timer1.stop();
                time1 = timer1.read();
                timer1.reset();
                }
                else time1 = 0.1;
                timer1.start();
                timer1_init = 1;
                
                for(i=0;i<5;i++){
                    sai_time[i] = time1*(i+1);
                }
                for(i=0;i<4;i++){
                    sai_hensa[i] = sai_hensa[i+1];
                }
                
                
                
                if(lr_on == 1 && lr_off == 1){
                    lr_counter ++;
                    kitai_angle = angle;
                    //con_theta = 0;
                    if(lr_counter > LR_YOKU){
                        lr_on = 0;
                        lr_off = 0;
                        lr_counter = 0;
                    }                    
                }
                
                if(ButtonState & (0x0001 << BUTTONTRIANGEL)){ //triangleで角度を初期化
                    axis_angle = raw_angle; //座標軸の設定
                    kitai_angle = raw_angle - axis_angle; //機体の維持角度
                    
                }
                
                
                hensa = angle - kitai_angle;
                if(hensa > 180)hensa -= 360;
                if(hensa < -180)hensa += 360;
                //if(hensa>60||hensa<-60)hensa = 0;
                
                proportion = hensa;
                sai_hensa[4] = hensa;
                //diff = sai1(sai_time,sai_hensa);
                diff = gyro_double;
                
               
                if(proportion < 4 && proportion > -4){ 
                    con_theta = -1*KP*proportion -1*KD*diff; //機体の維持角度と現在の角度の差
                }
                else{
                    con_theta = -1*KP2*proportion -1*KD2*diff;
                }
                
                
                
                
                
                //ボタンの処理
                //u16 ButtonState =  ((ps3report*)(data + 1))->ButtonState;
                if(start_flag == 1){
                if(ButtonState & 0x2000) {  //circleで右を振る
                air1=1;
                }
                if(air1 == 1){
                    raketto_time1 += time1;
                    if(raketto_time1 > 1){
                        raketto_time1 = 0;
                        air1 = 0;
                    }    
                }
                if(ButtonState & 0x8000) {  //squareで左を振る
                air2=1;
                }
                if(air2 == 1){
                    raketto_time2 += time1;
                    if(raketto_time2 > 1){
                        raketto_time2 = 0;
                        air2 = 0;
                    }    
                }
                if(ButtonState & 0x4000) {  //crassで両方戻す
                air1=0;air2=0;
                }                
                }
                
                if(ButtonState & (1 << BUTTONUP)){
                    up_state = 1;
                }
                else{
                    if(up_state == 1){
                    up_state = 0;
                    c_accel += 500;
                    if(c_accel > 6000) c_accel = 6000;
                    }
                }
                if(ButtonState & (1 << BUTTONDOWN)){
                    down_state = 1;
                }
                else{
                    if(down_state == 1){
                    down_state = 0;
                    c_accel -= 500;
                    if(c_accel<1000) c_accel = 1000;
                    }
                }
                if(ButtonState & (1 << BUTTONSTART)){
                    start_state = 1;
                    led1 = 1;
                }
                else{
                    //if(start_time < 0.5) start_state = 0;
                    if(start_state == 1){
                        if(start_flag == 0)start_flag = 1;
                        else{start_flag = 0;}
                        start_state = 0;
                        start_time = 0;
                    }
                    led1 = 0;
                }
                start_time += time1;
                if(start_flag == 0){
                    axis_angle = raw_angle; //座標軸の設定
                    kitai_angle = raw_angle - axis_angle; //機体の維持角度
                    led1 = 0;
                }
                else{led1 = 1;}
                
                //LeftStickの処理
                //if(LSX>=115&&LSX<=141) con_x = 0;
                con_x = LSX - LSXcent;
                //if(LSY>=115&&LSY<=141) con_y = 0;
                con_y = LSYcent - LSY;
                
                math1 = pow(con_x,2.0)+pow(con_y,2.0);
                math1 = sqrt(math1);
                
                if(math1 < 30){
                    con_x = 0; con_y = 0;
                }
                
                    
                    
                

                //LRの処理
                
                if((ButtonState & 0x0400)){             //L1
                    con_theta = KAITEN_VAL;
                    kitai_angle = angle; //機体の維持角度
                    if(angle>30){
                        con_theta = -1*KP*proportion -1*KD*diff;
                    }
                    lr_on = 1;
                }else if(ButtonState & 0x0100){         //L2
                    val_ratio = 0.5;                   
                }
                else if(ButtonState & 0x0800){         //R1
                    con_theta = -1*KAITEN_VAL;
                    kitai_angle = angle;
                    if(angle<-30){
                        con_theta = -1*KP*proportion -1*KD*diff;
                    }
                    lr_on = 1;
                }else if(ButtonState & 0x0200){         //R2            
                }else{
                    
                    val_ratio = 1.0;
                    if(lr_on == 1)lr_off = 1;                    
                }
                
                
                
                //ベクトル変換
                tar_val[3] = SCALE * val_ratio * ((cos2)*con_x -    sin2*con_y) + SCALE * con_theta;
                tar_val[2] = SCALE * val_ratio * ((-1*cos2)*con_y + (-1*sin2)*con_x) + SCALE * con_theta;
                tar_val[1] = SCALE * val_ratio * (   -1*cos2*con_x + sin2*con_y) + SCALE * con_theta;
                tar_val[0] = SCALE * val_ratio * (   cos2*con_y +    sin2*con_x) + SCALE * con_theta; 
                //最大値の処理
                Saidaika(tar_val);
                
                          
                Filter(tar_val);
                
               // c_accel = 500;
                AutoAccel(tar_val,real_val,val,time1,c_accel);
                
                if(start_flag == 0){
                    for(int i=0; i<4; i++) val[i] = 0;
                }
                
                for(int i=0; i<4; i++){
                    send_val[i] = val[i];
                }
                
                SendDAC(send_val);
                
                if(ButtonState & 0x0200){         //R2
                    static int accel_counter2;
                    double  buffx1;
                    accel_counter2++;
                    if(avr_accel_x<0)buffx1 = -1*avr_accel_x;
                    else buffx1 = avr_accel_x; 
                    for(i=0;i<AVR_ACCEL-1;i++){
                    accel_array[i] = accel_array[i+1];
                    }
                    accel_array[AVR_ACCEL-1] = buffx1;
                    c_accel += 10;
                    if(accel_counter2 > 5){
                        if(accel_array[0] > accel_array[AVR_ACCEL-1]) c_accel -= 300;
                        else c_accel += 50;
                        accel_counter2 = 0;
                    }
                    if(c_accel < 1000)c_accel = 1000;
                    if(c_accel > 6000)c_accel = 6000;
                }
                
                
                
                
                serial_counter ++;
                
               
#ifdef DEBUG_MODE                
                if(serial_counter > REFRESHRATE_PRINTF){
                printf("angle %f",angle*180/M_PI);
                printf("\t avr_accel_x %f",avr_accel_x);
                printf("\t math1 %f",math1);
                printf("\t c_accel %f",c_accel);
                //printf("\t con_rtheta %f",con_rtheta*180/M_PI);
                printf("\t kitai_angle %f",kitai_angle*180/M_PI);
                printf("\n");
                
                
                printf("data[0] %f",val[0]);
                printf("\t");
                printf("data[1] %f",val[1]);
                printf("\t");
                printf("data[2] %f",val[2]);
                printf("\t");
                printf("data[3] %f",val[3]);
                printf("\n");
                
                serial_counter = 0;
                }
#endif
                
                
                





                
                //////////////////////////////
                //断線時安全対策用
                //////////////////////////////
//                accel[accel_nowsub]=((ps3report*)(data + 1))->AccelX;
//                
//                if(accel[0]==accel[1] && accel[0]==accel[2] && accel[0]==accel[3] && accel[0]==accel[4]){
//                    data[0]=data[1]=data[2]=data[3]=0;
//                }
//                
//                if(accel_nowsub==4) accel_nowsub=0;
//                else accel_nowsub++;
                //// end of 安全対策 //////////

                 
                //////////////////////////////
                //send to DAC
                //////////////////////////////
                //DCA1_A
                
                
}
#endif


void AutoAccel(double *tar_data, double *real_data, double *data, double roop_time, double accel){
    static int max_range, state[4];
    static double  max[4], mini[4], range[4]={1,1,1,1};
    
    for(int i=0; i<4; i++){
        if(tar_data[i] >= real_data[i]){
            if(state[i] == 0){
                mini[i] = real_data[i];
                range[i] = tar_data[i] - mini[i];
            }
            if(state[i] == 1){
                range[i] = tar_data[i] - mini[i];
            }
            state[i] = 1;
        }
        else{
            if(state[i] == 1){
                max[i] = real_data[i];
                range[i] = max[i] - tar_data[i];
            }
            if(state[i] == 0){
                range[i] = max[i] - tar_data[i];
            }
            state[i] = 0;
        }
    }
    for(int i=0; i<4; i++){
        if(range[i] < 0) range[i] *= -1;
    }
    max_range = range[0];
    for(int i=1; i<4; i++){
        if(max_range < range[i]) max_range = range[i];
    }
    
    if(max_range == 0){
        for(int i=0; i<4; i++){
            range[i] = 0;
        }
    }
    else {
        for(int i=0; i<4; i++){
            range[i] = range[i] / max_range;
        }
    }
    
    
    for(int i=0; i<4; i++){
        if(tar_data[i] - real_data[i] > 0){
            if(real_data[i] > 0) data[i] += range[i]*accel*roop_time;
            else data[i] += 1*range[i]*accel*roop_time;
        } 
        if(tar_data[i] - real_data[i] < 0){
            if(real_data[i] < 0) data[i] -= range[i]*accel*roop_time;
            else data[i] -= 1*range[i]*accel*roop_time;
        }
        if(real_data[i] - tar_data[i] < range[i]*accel*roop_time && real_data[i] - tar_data[i] > -1*range[i]*accel*roop_time){
            data[i] = tar_data[i];
        }
    }
}

void Saidaika(double *data)
{
    double max = data[0];
    for(int i=1; i<4; i++){
        if(data[i] > max) max = data[i];
    }
    if(max > 4095){
        for(int i=0; i<4; i++){
            data[i] = data[i]*4095/max;
        }
    }
    double mini = data[0];
    for(int i=1; i<4; i++){
        if(data[i] < mini) mini = data[i];
    }
    if(mini < -4095){
        for(int i=0; i<4; i++){
            data[i] = data[i]*4095/(-1*mini);
        }
    }
}

void Filter(double *data)
{
    for(int i=0; i<4; i++){
        if(data[i] < CUTOFF && data[i] > -1*CUTOFF) data[i] = 0;
    }
}

void SendDAC(int *data)
{
    dac_data=0;
    if(data[0]>0)dac_data += data[0];
    dac_data += (1<<ga);//gain1x
    dac_data += (1<<shdn);//anarog on
        
    ldac1=1;
    ss1=0;
    wait_us(10);
    spi.write(dac_data);
    wait_us(10);
    ss1=1;
    ldac1=0;
    //DAC1_B
    dac_data=0;
    if(data[0]<0)dac_data -= data[0];
    dac_data += (1<<ab);//OUTb
    dac_data += (1<<ga);//gain1x
    dac_data += (1<<shdn);//anarog on
        
    ldac1=1;
    ss1=0;
    wait_us(10);
    spi.write(dac_data);
    wait_us(10);
    ss1=1;
    ldac1=0;
    //DCA2_A
    dac_data=0;
    if(data[1]>0)dac_data += data[1];
    dac_data += (1<<ga);//gain1x
    dac_data += (1<<shdn);//anarog on
        
    ldac2=1;
    ss2=0;
    wait_us(10);
    spi.write(dac_data);
    wait_us(10);
    ss2=1;
    ldac2=0;
    //DAC2_B
    dac_data=0;
    if(data[1]<0)dac_data -= data[1];
    dac_data += (1<<ab);//OUTb
    dac_data += (1<<ga);//gain1x
    dac_data += (1<<shdn);//anarog on
        
    ldac2=1;
    ss2=0;
    wait_us(10);
    spi.write(dac_data);
    wait_us(10);
    ss2=1;
    ldac2=0;
    //DCA3_A
    dac_data=0;
    if(data[2]>0)dac_data += data[2];
    dac_data += (1<<ga);//gain1x
    dac_data += (1<<shdn);//anarog on
        
    ldac3=1;
    ss3=0;
    wait_us(10);
    spi.write(dac_data);
    wait_us(10);
    ss3=1;
    ldac3=0;
    //DAC3_B
    dac_data=0;
    if(data[2]<0)dac_data -= data[2];
    dac_data += (1<<ab);//OUTb
    dac_data += (1<<ga);//gain1x
    dac_data += (1<<shdn);//anarog on
        
    ldac3=1;
    ss3=0;
    wait_us(10);
    spi.write(dac_data);
    wait_us(10);
    ss3=1;
    ldac3=0;
    //DCA4_A
    dac_data=0;
    if(data[3]>0)dac_data += data[3];
    dac_data += (1<<ga);//gain1x
    dac_data += (1<<shdn);//anarog on
        
    ldac4=1;
    ss4=0;
    wait_us(10);
    spi.write(dac_data);
    wait_us(10);
    ss4=1;
    ldac4=0;
    
    //DAC4_B
    dac_data=0;
    if(data[3]<0)dac_data -= data[3];
    dac_data += (1<<ab);//OUTb
    dac_data += (1<<ga);//gain1x
    dac_data += (1<<shdn);//anarog on
        
    ldac4=1;
    ss4=0;
    wait_us(10);
    spi.write(dac_data);
    wait_us(10);
    ss4=1;
    ldac4=0;
    //END of send to DAC
}


int TypeDivider(void *pType, char byte[]){
  short size = sizeof(pType);
  if(size > sizeof(byte)){
    return -1;                        //Error
  }
  for(short i=0;i<size;i++){
    byte[i] = *(( (char*)pType ) + i);
  }
  return 0;
}

int TypeConstructor(void *pType, char byte[]){
    short size = sizeof(pType);
    for(short i=0;i<size;i++){
         *(( (char*)pType ) + i) = byte[i];
    }
    return 0;
}    

double sai1(double x[],double y[])
{
        
        int i;
        double a0,a1,p,q;
        double A00,A01,A02,A11,A12;
 


        A00=A01=A02=A11=A12=0.0;
 
        for (i=0;i<5;i++) {
                A00+=1.0;
                A01+=x[i];
                A02+=y[i];
                A11+=x[i]*x[i];
                A12+=x[i]*y[i];
        }
 
/*１次式の係数の計算*/
        a0=(A02*A11-A01*A12)/(A00*A11-A01*A01);
        a1=(A00*A12-A01*A02)/(A00*A11-A01*A01);
 

 
/*gnuplotでグラフ表示のために
計算で得られた１次式でデータ近辺のグラフデータを保存*/
        return a1;
}