Diff: User.cpp

Revision:

0:0805c5a1b328

Child:

1:7b9e3032bb7b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/User.cpp	Sat Apr 18 06:27:36 2015 +0000
@@ -0,0 +1,749 @@
+
+
+
+#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;
+}
\ No newline at end of file