Diff: User.cpp

Revision:

1:7b9e3032bb7b

Parent:

0:0805c5a1b328

Child:

3:4d19e16fdf4c

--- a/User.cpp	Sat Apr 18 06:27:36 2015 +0000
+++ b/User.cpp	Mon May 18 03:50:39 2015 +0000
@@ -24,23 +24,25 @@
 #define RSYcent 128
 
 //Ticker ticker1;
-Timer timer1;
+//Timer timer1;
+SPI slave(p5, p6, p7); 
 CAN can1(p9, p10);
 CANMessage msg;  
 // pin settings
-DigitalOut air1(p5);
-DigitalOut air2(p6);
+//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);
+
+//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);
+//DigitalOut ldac1(p21);
+//DigitalOut ldac2(p22);
+//DigitalOut ldac3(p23);
+//DigitalOut ldac4(p24);
 // LED settings//
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
@@ -85,8 +87,9 @@
 
 
 void UserLoopSetting(){
-    spi.format(16,0);
-    spi.frequency(1000000);//1M
+    //spi.format(16,0);
+    //spi.frequency(1000000);//1M
+    /*
     ss1=1;
     ss2=1;
     ss3=1;
@@ -95,11 +98,19 @@
     ldac2=1;
     ldac3=1;
     ldac4=1;
+    */
+    can1.frequency(125000);
+    
+    slave.format(16,0);
+    slave.frequency(1000000);
+    
 }
 
 void UserLoop(char n,const u8* data){
                 //PS3conSTATE--->value
                 u16 ButtonState;
+                char Send_data[8] = {};
+                u16 spi_send_data[10] = {};
                 if(n==0){//有線Ps3USB.cpp
                     RSX = ((ps3report*)data)->RightStickX;
                     RSY = ((ps3report*)data)->RightStickY;
@@ -119,243 +130,67 @@
                     //ボタンの処理
                     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;
+                Send_data[0] = RSX;
+                Send_data[1] = RSY;
+                Send_data[2] = LSX;
+                Send_data[3] = LSY;
+                Send_data[4] = BSU;
+                Send_data[5] = BSL;
+                Send_data[6] =  ButtonState       & 0xFF;
+                Send_data[7] = (ButtonState >> 8) & 0xFF;
+                can1.write(CANMessage(100, &Send_data[0], 8));
+                
+                static int can1_counter = 0;
+                can1_counter ++;
+                if(can1_counter > 100){
+                    led1 = !led1;
+                    can1_counter = 0;   
+                }
+                /*
+                spi_send_data[0] = ButtonState;
+                spi_send_data[1] = RSX;
+                spi_send_data[1] = (spi_send_data[1] << 8) & 0xFF00;
+                spi_send_data[1] = spi_send_data[1] | RSY;
+                spi_send_data[2] = RSX;
+                spi_send_data[2] = (spi_send_data[2] << 8) & 0xFF00;
+                spi_send_data[2] = spi_send_data[2] | RSY;
+                */
+                /*
+                if(slave.receive()){
+                    int recive = slave.read();
+                    switch(recive){
+                        case 1:
+                            slave.reply(spi_send_data[0]);
+                            break; 
+                        case 2:
+                            slave.reply(spi_send_data[1]);
+                            break; 
+                        case 3:
+                            slave.reply(spi_send_data[2]);
+                            break; 
+                    }
+                }
+                */
                 
                 
-                for(int i = 0; i < 4; i++){
-                    real_val[i] = val[i];
-                }
+                
+                //slave.write(1);
+                
                 
 
-                if(caninit_triger == 0){
-                    can1.frequency(125000);
-                    caninit_triger = 1;  
-                }
+                
                 
                 
                 
                 
                                              
-                can1.read(msg); //CAN読み取り
+                //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;
-                }
                 
                     
                     
@@ -363,77 +198,6 @@
 
                 //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 ++;
@@ -467,283 +231,8 @@
                 
 
 
-
-
-
-                
-                //////////////////////////////
-                //断線時安全対策用
-                //////////////////////////////
-//                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