ROBOSTEP_3rd_SHARE / Mbed 2 deprecated PS3conOut2

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 PS3conOut by ROBOSTEP_3rd_SHARE

User.cpp@0:0805c5a1b328, 2015-04-18 (annotated)

Committer:
ideguti
Date:
Sat Apr 18 06:27:36 2015 +0000
Revision:
0:0805c5a1b328
Child:
1:7b9e3032bb7b
koueki you no program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ideguti 0:0805c5a1b328 1
ideguti 0:0805c5a1b328 2
ideguti 0:0805c5a1b328 3
ideguti 0:0805c5a1b328 4 #include <math.h>
ideguti 0:0805c5a1b328 5 #include "Utils.h"
ideguti 0:0805c5a1b328 6 #include "USBHost.h"
ideguti 0:0805c5a1b328 7 #include "hci.h"
ideguti 0:0805c5a1b328 8 #include "ps3.h"
ideguti 0:0805c5a1b328 9 #include "TestShell.h"
ideguti 0:0805c5a1b328 10 #include "User.h"
ideguti 0:0805c5a1b328 11 #include "caninit.h"
ideguti 0:0805c5a1b328 12 #include "mbed.h"
ideguti 0:0805c5a1b328 13 #ifndef M_PI
ideguti 0:0805c5a1b328 14 #define M_PI 3.14159265f
ideguti 0:0805c5a1b328 15 #endif
ideguti 0:0805c5a1b328 16 #ifdef common
ideguti 0:0805c5a1b328 17 #define ab 15
ideguti 0:0805c5a1b328 18 #define buf 14
ideguti 0:0805c5a1b328 19 #define ga 13
ideguti 0:0805c5a1b328 20 #define shdn 12
ideguti 0:0805c5a1b328 21 #define LSXcent 128
ideguti 0:0805c5a1b328 22 #define LSYcent 128
ideguti 0:0805c5a1b328 23 #define RSXcent 128
ideguti 0:0805c5a1b328 24 #define RSYcent 128
ideguti 0:0805c5a1b328 25
ideguti 0:0805c5a1b328 26 //Ticker ticker1;
ideguti 0:0805c5a1b328 27 Timer timer1;
ideguti 0:0805c5a1b328 28 CAN can1(p9, p10);
ideguti 0:0805c5a1b328 29 CANMessage msg;
ideguti 0:0805c5a1b328 30 // pin settings
ideguti 0:0805c5a1b328 31 DigitalOut air1(p5);
ideguti 0:0805c5a1b328 32 DigitalOut air2(p6);
ideguti 0:0805c5a1b328 33 //SPI settings//
ideguti 0:0805c5a1b328 34 SPI spi(p11,p12,p13);
ideguti 0:0805c5a1b328 35 DigitalOut ss1(p20);
ideguti 0:0805c5a1b328 36 DigitalOut ss2(p19);
ideguti 0:0805c5a1b328 37 DigitalOut ss3(p18);
ideguti 0:0805c5a1b328 38 DigitalOut ss4(p17);
ideguti 0:0805c5a1b328 39 //PINsettings
ideguti 0:0805c5a1b328 40 DigitalOut ldac1(p21);
ideguti 0:0805c5a1b328 41 DigitalOut ldac2(p22);
ideguti 0:0805c5a1b328 42 DigitalOut ldac3(p23);
ideguti 0:0805c5a1b328 43 DigitalOut ldac4(p24);
ideguti 0:0805c5a1b328 44 // LED settings//
ideguti 0:0805c5a1b328 45 DigitalOut led1(LED1);
ideguti 0:0805c5a1b328 46 DigitalOut led2(LED2);
ideguti 0:0805c5a1b328 47 DigitalOut led3(LED3);
ideguti 0:0805c5a1b328 48 DigitalOut led4(LED4);
ideguti 0:0805c5a1b328 49 //
ideguti 0:0805c5a1b328 50 int serial_counter = 0;
ideguti 0:0805c5a1b328 51 int dac_data=0;
ideguti 0:0805c5a1b328 52 double debug_val = 0, debug_val2 = 0;
ideguti 0:0805c5a1b328 53 double real_val[4] = {};
ideguti 0:0805c5a1b328 54 double tar_val[4] = {};
ideguti 0:0805c5a1b328 55 double val[4] = {};
ideguti 0:0805c5a1b328 56 int send_val[4] = {};
ideguti 0:0805c5a1b328 57 double c_accel=5000;
ideguti 0:0805c5a1b328 58 char teisu=13;//23;
ideguti 0:0805c5a1b328 59 char can_send_data[8];
ideguti 0:0805c5a1b328 60
ideguti 0:0805c5a1b328 61 int con_x;
ideguti 0:0805c5a1b328 62 int con_y;
ideguti 0:0805c5a1b328 63 int con_theta;
ideguti 0:0805c5a1b328 64 double val_ratio, math1;
ideguti 0:0805c5a1b328 65 double sai_time[5]={}, sai_hensa[5]={};
ideguti 0:0805c5a1b328 66
ideguti 0:0805c5a1b328 67 long angle_ulong;
ideguti 0:0805c5a1b328 68 float raw_angle;
ideguti 0:0805c5a1b328 69 float angle;
ideguti 0:0805c5a1b328 70 float axis_angle = 0;
ideguti 0:0805c5a1b328 71 double sin2, cos2;
ideguti 0:0805c5a1b328 72 float kitai_angle = 0;
ideguti 0:0805c5a1b328 73 bool caninit_triger = 0;
ideguti 0:0805c5a1b328 74 int can_counter = 0;
ideguti 0:0805c5a1b328 75 double avr_accel_x, avr_accel_y;
ideguti 0:0805c5a1b328 76
ideguti 0:0805c5a1b328 77
ideguti 0:0805c5a1b328 78 //
ideguti 0:0805c5a1b328 79 u8 RSX,RSY,LSX,LSY,BSU,BSL;
ideguti 0:0805c5a1b328 80 //コントローラ断線時対策用
ideguti 0:0805c5a1b328 81 u8 accel[5];
ideguti 0:0805c5a1b328 82 u8 accel_nowsub=0;
ideguti 0:0805c5a1b328 83 //////////
ideguti 0:0805c5a1b328 84 char cnt=0;
ideguti 0:0805c5a1b328 85
ideguti 0:0805c5a1b328 86
ideguti 0:0805c5a1b328 87 void UserLoopSetting(){
ideguti 0:0805c5a1b328 88 spi.format(16,0);
ideguti 0:0805c5a1b328 89 spi.frequency(1000000);//1M
ideguti 0:0805c5a1b328 90 ss1=1;
ideguti 0:0805c5a1b328 91 ss2=1;
ideguti 0:0805c5a1b328 92 ss3=1;
ideguti 0:0805c5a1b328 93 ss4=1;
ideguti 0:0805c5a1b328 94 ldac1=1;
ideguti 0:0805c5a1b328 95 ldac2=1;
ideguti 0:0805c5a1b328 96 ldac3=1;
ideguti 0:0805c5a1b328 97 ldac4=1;
ideguti 0:0805c5a1b328 98 }
ideguti 0:0805c5a1b328 99
ideguti 0:0805c5a1b328 100 void UserLoop(char n,const u8* data){
ideguti 0:0805c5a1b328 101 //PS3conSTATE--->value
ideguti 0:0805c5a1b328 102 u16 ButtonState;
ideguti 0:0805c5a1b328 103 if(n==0){//有線Ps3USB.cpp
ideguti 0:0805c5a1b328 104 RSX = ((ps3report*)data)->RightStickX;
ideguti 0:0805c5a1b328 105 RSY = ((ps3report*)data)->RightStickY;
ideguti 0:0805c5a1b328 106 LSX = ((ps3report*)data)->LeftStickX;
ideguti 0:0805c5a1b328 107 LSY = ((ps3report*)data)->LeftStickY;
ideguti 0:0805c5a1b328 108 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 109 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
ideguti 0:0805c5a1b328 110 //ボタンの処理
ideguti 0:0805c5a1b328 111 ButtonState = ((ps3report*)data)->ButtonState;
ideguti 0:0805c5a1b328 112 }else {//無線TestShell.cpp
ideguti 0:0805c5a1b328 113 RSX = ((ps3report*)(data + 1))->RightStickX;
ideguti 0:0805c5a1b328 114 RSY = ((ps3report*)(data + 1))->RightStickY;
ideguti 0:0805c5a1b328 115 LSX = ((ps3report*)(data + 1))->LeftStickX;
ideguti 0:0805c5a1b328 116 LSY = ((ps3report*)(data + 1))->LeftStickY;
ideguti 0:0805c5a1b328 117 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
ideguti 0:0805c5a1b328 118 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
ideguti 0:0805c5a1b328 119 //ボタンの処理
ideguti 0:0805c5a1b328 120 ButtonState = ((ps3report*)(data + 1))->ButtonState;
ideguti 0:0805c5a1b328 121 }
ideguti 0:0805c5a1b328 122
ideguti 0:0805c5a1b328 123
ideguti 0:0805c5a1b328 124
ideguti 0:0805c5a1b328 125 static long gyro_long;
ideguti 0:0805c5a1b328 126 static short accel_long_x, accel_long_y;
ideguti 0:0805c5a1b328 127 static double gyro_double, accel_double_x, accel_double_y;
ideguti 0:0805c5a1b328 128 static double accel_x[AVR_ACCEL]={}, accel_y[AVR_ACCEL]={};
ideguti 0:0805c5a1b328 129 float proportion, hensa;
ideguti 0:0805c5a1b328 130 int i;
ideguti 0:0805c5a1b328 131 double time1, diff;
ideguti 0:0805c5a1b328 132 static bool timer1_init = 0;
ideguti 0:0805c5a1b328 133 static bool lr_on, lr_off;
ideguti 0:0805c5a1b328 134 static int lr_counter;
ideguti 0:0805c5a1b328 135 static double raketto_time1, raketto_time2;
ideguti 0:0805c5a1b328 136 static double accel_array[AVR_ACCEL]={};
ideguti 0:0805c5a1b328 137 static bool down_state = 0;
ideguti 0:0805c5a1b328 138 static bool up_state = 0;
ideguti 0:0805c5a1b328 139 static bool start_state = 0, start_flag = 0;
ideguti 0:0805c5a1b328 140 static double start_time = 0;
ideguti 0:0805c5a1b328 141
ideguti 0:0805c5a1b328 142
ideguti 0:0805c5a1b328 143 for(int i = 0; i < 4; i++){
ideguti 0:0805c5a1b328 144 real_val[i] = val[i];
ideguti 0:0805c5a1b328 145 }
ideguti 0:0805c5a1b328 146
ideguti 0:0805c5a1b328 147
ideguti 0:0805c5a1b328 148 if(caninit_triger == 0){
ideguti 0:0805c5a1b328 149 can1.frequency(125000);
ideguti 0:0805c5a1b328 150 caninit_triger = 1;
ideguti 0:0805c5a1b328 151 }
ideguti 0:0805c5a1b328 152
ideguti 0:0805c5a1b328 153
ideguti 0:0805c5a1b328 154
ideguti 0:0805c5a1b328 155
ideguti 0:0805c5a1b328 156
ideguti 0:0805c5a1b328 157 can1.read(msg); //CAN読み取り
ideguti 0:0805c5a1b328 158
ideguti 0:0805c5a1b328 159 if(msg.id == 700){
ideguti 0:0805c5a1b328 160 static int angle_led;
ideguti 0:0805c5a1b328 161 angle_led ++;
ideguti 0:0805c5a1b328 162 if(angle_led > 2){
ideguti 0:0805c5a1b328 163 led2 = !led2;
ideguti 0:0805c5a1b328 164 angle_led = 0;
ideguti 0:0805c5a1b328 165 }
ideguti 0:0805c5a1b328 166 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]);
ideguti 0:0805c5a1b328 167 raw_angle = -1.0*angle_ulong*180.0/(M_PI*100000.0);
ideguti 0:0805c5a1b328 168 angle = raw_angle - axis_angle; // 軸からの角度を計算
ideguti 0:0805c5a1b328 169 if(angle < -180) angle += 360;
ideguti 0:0805c5a1b328 170 if(angle > 180) angle -= 360;
ideguti 0:0805c5a1b328 171 sin2 = sin((45-angle)*M_PI/180);
ideguti 0:0805c5a1b328 172 cos2 = cos((45-angle)*M_PI/180);
ideguti 0:0805c5a1b328 173 }
ideguti 0:0805c5a1b328 174 if(msg.id == 700){
ideguti 0:0805c5a1b328 175 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]);
ideguti 0:0805c5a1b328 176 gyro_double = gyro_long/100000.0;
ideguti 0:0805c5a1b328 177 }
ideguti 0:0805c5a1b328 178 if(msg.id == 710){
ideguti 0:0805c5a1b328 179 static int accel_led;
ideguti 0:0805c5a1b328 180 accel_led ++;
ideguti 0:0805c5a1b328 181 if(accel_led > 2){
ideguti 0:0805c5a1b328 182 led3 = !led3;
ideguti 0:0805c5a1b328 183 accel_led = 0;
ideguti 0:0805c5a1b328 184 }
ideguti 0:0805c5a1b328 185 accel_long_x = (msg.data[0] << 8) | (msg.data[1] );
ideguti 0:0805c5a1b328 186 accel_long_y = ((unsigned long)msg.data[0+2] << 8) | ((unsigned long)msg.data[1+2]);
ideguti 0:0805c5a1b328 187 accel_double_x = accel_long_x/100.0;
ideguti 0:0805c5a1b328 188 accel_double_y = accel_long_y/100.0;
ideguti 0:0805c5a1b328 189 }
ideguti 0:0805c5a1b328 190
ideguti 0:0805c5a1b328 191 float flt_buff = 0;
ideguti 0:0805c5a1b328 192 flt_buff = real_val[0];
ideguti 0:0805c5a1b328 193 TypeDivider( &flt_buff, can_send_data);
ideguti 0:0805c5a1b328 194
ideguti 0:0805c5a1b328 195 //can1.write(CANMessage(300, &can_send_data[0], 4));
ideguti 0:0805c5a1b328 196
ideguti 0:0805c5a1b328 197
ideguti 0:0805c5a1b328 198
ideguti 0:0805c5a1b328 199
ideguti 0:0805c5a1b328 200 for(i=0;i<AVR_ACCEL-1;i++){
ideguti 0:0805c5a1b328 201 accel_x[i] = accel_x[i+1];
ideguti 0:0805c5a1b328 202 }
ideguti 0:0805c5a1b328 203 for(i=0;i<AVR_ACCEL-1;i++){
ideguti 0:0805c5a1b328 204 accel_y[i] = accel_y[i+1];
ideguti 0:0805c5a1b328 205 }
ideguti 0:0805c5a1b328 206 accel_x[AVR_ACCEL-1] = accel_double_x;
ideguti 0:0805c5a1b328 207 accel_y[AVR_ACCEL-1] = accel_double_y;
ideguti 0:0805c5a1b328 208
ideguti 0:0805c5a1b328 209 for(i=0;i<AVR_ACCEL;i++){
ideguti 0:0805c5a1b328 210 avr_accel_x += accel_x[i];
ideguti 0:0805c5a1b328 211 }
ideguti 0:0805c5a1b328 212 avr_accel_x = avr_accel_x/(AVR_ACCEL+1);
ideguti 0:0805c5a1b328 213 for(i=0;i<AVR_ACCEL;i++){
ideguti 0:0805c5a1b328 214 avr_accel_y += accel_y[i];
ideguti 0:0805c5a1b328 215 }
ideguti 0:0805c5a1b328 216 avr_accel_y = avr_accel_y/(AVR_ACCEL+1);
ideguti 0:0805c5a1b328 217
ideguti 0:0805c5a1b328 218
ideguti 0:0805c5a1b328 219 if(timer1_init == 1){
ideguti 0:0805c5a1b328 220 timer1.stop();
ideguti 0:0805c5a1b328 221 time1 = timer1.read();
ideguti 0:0805c5a1b328 222 timer1.reset();
ideguti 0:0805c5a1b328 223 }
ideguti 0:0805c5a1b328 224 else time1 = 0.1;
ideguti 0:0805c5a1b328 225 timer1.start();
ideguti 0:0805c5a1b328 226 timer1_init = 1;
ideguti 0:0805c5a1b328 227
ideguti 0:0805c5a1b328 228 for(i=0;i<5;i++){
ideguti 0:0805c5a1b328 229 sai_time[i] = time1*(i+1);
ideguti 0:0805c5a1b328 230 }
ideguti 0:0805c5a1b328 231 for(i=0;i<4;i++){
ideguti 0:0805c5a1b328 232 sai_hensa[i] = sai_hensa[i+1];
ideguti 0:0805c5a1b328 233 }
ideguti 0:0805c5a1b328 234
ideguti 0:0805c5a1b328 235
ideguti 0:0805c5a1b328 236
ideguti 0:0805c5a1b328 237 if(lr_on == 1 && lr_off == 1){
ideguti 0:0805c5a1b328 238 lr_counter ++;
ideguti 0:0805c5a1b328 239 kitai_angle = angle;
ideguti 0:0805c5a1b328 240 //con_theta = 0;
ideguti 0:0805c5a1b328 241 if(lr_counter > LR_YOKU){
ideguti 0:0805c5a1b328 242 lr_on = 0;
ideguti 0:0805c5a1b328 243 lr_off = 0;
ideguti 0:0805c5a1b328 244 lr_counter = 0;
ideguti 0:0805c5a1b328 245 }
ideguti 0:0805c5a1b328 246 }
ideguti 0:0805c5a1b328 247
ideguti 0:0805c5a1b328 248 if(ButtonState & (0x0001 << BUTTONTRIANGEL)){ //triangleで角度を初期化
ideguti 0:0805c5a1b328 249 axis_angle = raw_angle; //座標軸の設定
ideguti 0:0805c5a1b328 250 kitai_angle = raw_angle - axis_angle; //機体の維持角度
ideguti 0:0805c5a1b328 251
ideguti 0:0805c5a1b328 252 }
ideguti 0:0805c5a1b328 253
ideguti 0:0805c5a1b328 254
ideguti 0:0805c5a1b328 255 hensa = angle - kitai_angle;
ideguti 0:0805c5a1b328 256 if(hensa > 180)hensa -= 360;
ideguti 0:0805c5a1b328 257 if(hensa < -180)hensa += 360;
ideguti 0:0805c5a1b328 258 //if(hensa>60||hensa<-60)hensa = 0;
ideguti 0:0805c5a1b328 259
ideguti 0:0805c5a1b328 260 proportion = hensa;
ideguti 0:0805c5a1b328 261 sai_hensa[4] = hensa;
ideguti 0:0805c5a1b328 262 //diff = sai1(sai_time,sai_hensa);
ideguti 0:0805c5a1b328 263 diff = gyro_double;
ideguti 0:0805c5a1b328 264
ideguti 0:0805c5a1b328 265
ideguti 0:0805c5a1b328 266 if(proportion < 4 && proportion > -4){
ideguti 0:0805c5a1b328 267 con_theta = -1*KP*proportion -1*KD*diff; //機体の維持角度と現在の角度の差
ideguti 0:0805c5a1b328 268 }
ideguti 0:0805c5a1b328 269 else{
ideguti 0:0805c5a1b328 270 con_theta = -1*KP2*proportion -1*KD2*diff;
ideguti 0:0805c5a1b328 271 }
ideguti 0:0805c5a1b328 272
ideguti 0:0805c5a1b328 273
ideguti 0:0805c5a1b328 274
ideguti 0:0805c5a1b328 275
ideguti 0:0805c5a1b328 276
ideguti 0:0805c5a1b328 277 //ボタンの処理
ideguti 0:0805c5a1b328 278 //u16 ButtonState = ((ps3report*)(data + 1))->ButtonState;
ideguti 0:0805c5a1b328 279 if(start_flag == 1){
ideguti 0:0805c5a1b328 280 if(ButtonState & 0x2000) { //circleで右を振る
ideguti 0:0805c5a1b328 281 air1=1;
ideguti 0:0805c5a1b328 282 }
ideguti 0:0805c5a1b328 283 if(air1 == 1){
ideguti 0:0805c5a1b328 284 raketto_time1 += time1;
ideguti 0:0805c5a1b328 285 if(raketto_time1 > 1){
ideguti 0:0805c5a1b328 286 raketto_time1 = 0;
ideguti 0:0805c5a1b328 287 air1 = 0;
ideguti 0:0805c5a1b328 288 }
ideguti 0:0805c5a1b328 289 }
ideguti 0:0805c5a1b328 290 if(ButtonState & 0x8000) { //squareで左を振る
ideguti 0:0805c5a1b328 291 air2=1;
ideguti 0:0805c5a1b328 292 }
ideguti 0:0805c5a1b328 293 if(air2 == 1){
ideguti 0:0805c5a1b328 294 raketto_time2 += time1;
ideguti 0:0805c5a1b328 295 if(raketto_time2 > 1){
ideguti 0:0805c5a1b328 296 raketto_time2 = 0;
ideguti 0:0805c5a1b328 297 air2 = 0;
ideguti 0:0805c5a1b328 298 }
ideguti 0:0805c5a1b328 299 }
ideguti 0:0805c5a1b328 300 if(ButtonState & 0x4000) { //crassで両方戻す
ideguti 0:0805c5a1b328 301 air1=0;air2=0;
ideguti 0:0805c5a1b328 302 }
ideguti 0:0805c5a1b328 303 }
ideguti 0:0805c5a1b328 304
ideguti 0:0805c5a1b328 305 if(ButtonState & (1 << BUTTONUP)){
ideguti 0:0805c5a1b328 306 up_state = 1;
ideguti 0:0805c5a1b328 307 }
ideguti 0:0805c5a1b328 308 else{
ideguti 0:0805c5a1b328 309 if(up_state == 1){
ideguti 0:0805c5a1b328 310 up_state = 0;
ideguti 0:0805c5a1b328 311 c_accel += 500;
ideguti 0:0805c5a1b328 312 if(c_accel > 6000) c_accel = 6000;
ideguti 0:0805c5a1b328 313 }
ideguti 0:0805c5a1b328 314 }
ideguti 0:0805c5a1b328 315 if(ButtonState & (1 << BUTTONDOWN)){
ideguti 0:0805c5a1b328 316 down_state = 1;
ideguti 0:0805c5a1b328 317 }
ideguti 0:0805c5a1b328 318 else{
ideguti 0:0805c5a1b328 319 if(down_state == 1){
ideguti 0:0805c5a1b328 320 down_state = 0;
ideguti 0:0805c5a1b328 321 c_accel -= 500;
ideguti 0:0805c5a1b328 322 if(c_accel<1000) c_accel = 1000;
ideguti 0:0805c5a1b328 323 }
ideguti 0:0805c5a1b328 324 }
ideguti 0:0805c5a1b328 325 if(ButtonState & (1 << BUTTONSTART)){
ideguti 0:0805c5a1b328 326 start_state = 1;
ideguti 0:0805c5a1b328 327 led1 = 1;
ideguti 0:0805c5a1b328 328 }
ideguti 0:0805c5a1b328 329 else{
ideguti 0:0805c5a1b328 330 //if(start_time < 0.5) start_state = 0;
ideguti 0:0805c5a1b328 331 if(start_state == 1){
ideguti 0:0805c5a1b328 332 if(start_flag == 0)start_flag = 1;
ideguti 0:0805c5a1b328 333 else{start_flag = 0;}
ideguti 0:0805c5a1b328 334 start_state = 0;
ideguti 0:0805c5a1b328 335 start_time = 0;
ideguti 0:0805c5a1b328 336 }
ideguti 0:0805c5a1b328 337 led1 = 0;
ideguti 0:0805c5a1b328 338 }
ideguti 0:0805c5a1b328 339 start_time += time1;
ideguti 0:0805c5a1b328 340 if(start_flag == 0){
ideguti 0:0805c5a1b328 341 axis_angle = raw_angle; //座標軸の設定
ideguti 0:0805c5a1b328 342 kitai_angle = raw_angle - axis_angle; //機体の維持角度
ideguti 0:0805c5a1b328 343 led1 = 0;
ideguti 0:0805c5a1b328 344 }
ideguti 0:0805c5a1b328 345 else{led1 = 1;}
ideguti 0:0805c5a1b328 346
ideguti 0:0805c5a1b328 347 //LeftStickの処理
ideguti 0:0805c5a1b328 348 //if(LSX>=115&&LSX<=141) con_x = 0;
ideguti 0:0805c5a1b328 349 con_x = LSX - LSXcent;
ideguti 0:0805c5a1b328 350 //if(LSY>=115&&LSY<=141) con_y = 0;
ideguti 0:0805c5a1b328 351 con_y = LSYcent - LSY;
ideguti 0:0805c5a1b328 352
ideguti 0:0805c5a1b328 353 math1 = pow(con_x,2.0)+pow(con_y,2.0);
ideguti 0:0805c5a1b328 354 math1 = sqrt(math1);
ideguti 0:0805c5a1b328 355
ideguti 0:0805c5a1b328 356 if(math1 < 30){
ideguti 0:0805c5a1b328 357 con_x = 0; con_y = 0;
ideguti 0:0805c5a1b328 358 }
ideguti 0:0805c5a1b328 359
ideguti 0:0805c5a1b328 360
ideguti 0:0805c5a1b328 361
ideguti 0:0805c5a1b328 362
ideguti 0:0805c5a1b328 363
ideguti 0:0805c5a1b328 364 //LRの処理
ideguti 0:0805c5a1b328 365
ideguti 0:0805c5a1b328 366 if((ButtonState & 0x0400)){ //L1
ideguti 0:0805c5a1b328 367 con_theta = KAITEN_VAL;
ideguti 0:0805c5a1b328 368 kitai_angle = angle; //機体の維持角度
ideguti 0:0805c5a1b328 369 if(angle>30){
ideguti 0:0805c5a1b328 370 con_theta = -1*KP*proportion -1*KD*diff;
ideguti 0:0805c5a1b328 371 }
ideguti 0:0805c5a1b328 372 lr_on = 1;
ideguti 0:0805c5a1b328 373 }else if(ButtonState & 0x0100){ //L2
ideguti 0:0805c5a1b328 374 val_ratio = 0.5;
ideguti 0:0805c5a1b328 375 }
ideguti 0:0805c5a1b328 376 else if(ButtonState & 0x0800){ //R1
ideguti 0:0805c5a1b328 377 con_theta = -1*KAITEN_VAL;
ideguti 0:0805c5a1b328 378 kitai_angle = angle;
ideguti 0:0805c5a1b328 379 if(angle<-30){
ideguti 0:0805c5a1b328 380 con_theta = -1*KP*proportion -1*KD*diff;
ideguti 0:0805c5a1b328 381 }
ideguti 0:0805c5a1b328 382 lr_on = 1;
ideguti 0:0805c5a1b328 383 }else if(ButtonState & 0x0200){ //R2
ideguti 0:0805c5a1b328 384 }else{
ideguti 0:0805c5a1b328 385
ideguti 0:0805c5a1b328 386 val_ratio = 1.0;
ideguti 0:0805c5a1b328 387 if(lr_on == 1)lr_off = 1;
ideguti 0:0805c5a1b328 388 }
ideguti 0:0805c5a1b328 389
ideguti 0:0805c5a1b328 390
ideguti 0:0805c5a1b328 391
ideguti 0:0805c5a1b328 392 //ベクトル変換
ideguti 0:0805c5a1b328 393 tar_val[3] = SCALE * val_ratio * ((cos2)*con_x - sin2*con_y) + SCALE * con_theta;
ideguti 0:0805c5a1b328 394 tar_val[2] = SCALE * val_ratio * ((-1*cos2)*con_y + (-1*sin2)*con_x) + SCALE * con_theta;
ideguti 0:0805c5a1b328 395 tar_val[1] = SCALE * val_ratio * ( -1*cos2*con_x + sin2*con_y) + SCALE * con_theta;
ideguti 0:0805c5a1b328 396 tar_val[0] = SCALE * val_ratio * ( cos2*con_y + sin2*con_x) + SCALE * con_theta;
ideguti 0:0805c5a1b328 397 //最大値の処理
ideguti 0:0805c5a1b328 398 Saidaika(tar_val);
ideguti 0:0805c5a1b328 399
ideguti 0:0805c5a1b328 400
ideguti 0:0805c5a1b328 401 Filter(tar_val);
ideguti 0:0805c5a1b328 402
ideguti 0:0805c5a1b328 403 // c_accel = 500;
ideguti 0:0805c5a1b328 404 AutoAccel(tar_val,real_val,val,time1,c_accel);
ideguti 0:0805c5a1b328 405
ideguti 0:0805c5a1b328 406 if(start_flag == 0){
ideguti 0:0805c5a1b328 407 for(int i=0; i<4; i++) val[i] = 0;
ideguti 0:0805c5a1b328 408 }
ideguti 0:0805c5a1b328 409
ideguti 0:0805c5a1b328 410 for(int i=0; i<4; i++){
ideguti 0:0805c5a1b328 411 send_val[i] = val[i];
ideguti 0:0805c5a1b328 412 }
ideguti 0:0805c5a1b328 413
ideguti 0:0805c5a1b328 414 SendDAC(send_val);
ideguti 0:0805c5a1b328 415
ideguti 0:0805c5a1b328 416 if(ButtonState & 0x0200){ //R2
ideguti 0:0805c5a1b328 417 static int accel_counter2;
ideguti 0:0805c5a1b328 418 double buffx1;
ideguti 0:0805c5a1b328 419 accel_counter2++;
ideguti 0:0805c5a1b328 420 if(avr_accel_x<0)buffx1 = -1*avr_accel_x;
ideguti 0:0805c5a1b328 421 else buffx1 = avr_accel_x;
ideguti 0:0805c5a1b328 422 for(i=0;i<AVR_ACCEL-1;i++){
ideguti 0:0805c5a1b328 423 accel_array[i] = accel_array[i+1];
ideguti 0:0805c5a1b328 424 }
ideguti 0:0805c5a1b328 425 accel_array[AVR_ACCEL-1] = buffx1;
ideguti 0:0805c5a1b328 426 c_accel += 10;
ideguti 0:0805c5a1b328 427 if(accel_counter2 > 5){
ideguti 0:0805c5a1b328 428 if(accel_array[0] > accel_array[AVR_ACCEL-1]) c_accel -= 300;
ideguti 0:0805c5a1b328 429 else c_accel += 50;
ideguti 0:0805c5a1b328 430 accel_counter2 = 0;
ideguti 0:0805c5a1b328 431 }
ideguti 0:0805c5a1b328 432 if(c_accel < 1000)c_accel = 1000;
ideguti 0:0805c5a1b328 433 if(c_accel > 6000)c_accel = 6000;
ideguti 0:0805c5a1b328 434 }
ideguti 0:0805c5a1b328 435
ideguti 0:0805c5a1b328 436
ideguti 0:0805c5a1b328 437
ideguti 0:0805c5a1b328 438
ideguti 0:0805c5a1b328 439 serial_counter ++;
ideguti 0:0805c5a1b328 440
ideguti 0:0805c5a1b328 441
ideguti 0:0805c5a1b328 442 #ifdef DEBUG_MODE
ideguti 0:0805c5a1b328 443 if(serial_counter > REFRESHRATE_PRINTF){
ideguti 0:0805c5a1b328 444 printf("angle %f",angle*180/M_PI);
ideguti 0:0805c5a1b328 445 printf("\t avr_accel_x %f",avr_accel_x);
ideguti 0:0805c5a1b328 446 printf("\t math1 %f",math1);
ideguti 0:0805c5a1b328 447 printf("\t c_accel %f",c_accel);
ideguti 0:0805c5a1b328 448 //printf("\t con_rtheta %f",con_rtheta*180/M_PI);
ideguti 0:0805c5a1b328 449 printf("\t kitai_angle %f",kitai_angle*180/M_PI);
ideguti 0:0805c5a1b328 450 printf("\n");
ideguti 0:0805c5a1b328 451
ideguti 0:0805c5a1b328 452
ideguti 0:0805c5a1b328 453 printf("data[0] %f",val[0]);
ideguti 0:0805c5a1b328 454 printf("\t");
ideguti 0:0805c5a1b328 455 printf("data[1] %f",val[1]);
ideguti 0:0805c5a1b328 456 printf("\t");
ideguti 0:0805c5a1b328 457 printf("data[2] %f",val[2]);
ideguti 0:0805c5a1b328 458 printf("\t");
ideguti 0:0805c5a1b328 459 printf("data[3] %f",val[3]);
ideguti 0:0805c5a1b328 460 printf("\n");
ideguti 0:0805c5a1b328 461
ideguti 0:0805c5a1b328 462 serial_counter = 0;
ideguti 0:0805c5a1b328 463 }
ideguti 0:0805c5a1b328 464 #endif
ideguti 0:0805c5a1b328 465
ideguti 0:0805c5a1b328 466
ideguti 0:0805c5a1b328 467
ideguti 0:0805c5a1b328 468
ideguti 0:0805c5a1b328 469
ideguti 0:0805c5a1b328 470
ideguti 0:0805c5a1b328 471
ideguti 0:0805c5a1b328 472
ideguti 0:0805c5a1b328 473
ideguti 0:0805c5a1b328 474 //////////////////////////////
ideguti 0:0805c5a1b328 475 //断線時安全対策用
ideguti 0:0805c5a1b328 476 //////////////////////////////
ideguti 0:0805c5a1b328 477 // accel[accel_nowsub]=((ps3report*)(data + 1))->AccelX;
ideguti 0:0805c5a1b328 478 //
ideguti 0:0805c5a1b328 479 // if(accel[0]==accel[1] && accel[0]==accel[2] && accel[0]==accel[3] && accel[0]==accel[4]){
ideguti 0:0805c5a1b328 480 // data[0]=data[1]=data[2]=data[3]=0;
ideguti 0:0805c5a1b328 481 // }
ideguti 0:0805c5a1b328 482 //
ideguti 0:0805c5a1b328 483 // if(accel_nowsub==4) accel_nowsub=0;
ideguti 0:0805c5a1b328 484 // else accel_nowsub++;
ideguti 0:0805c5a1b328 485 //// end of 安全対策 //////////
ideguti 0:0805c5a1b328 486
ideguti 0:0805c5a1b328 487
ideguti 0:0805c5a1b328 488 //////////////////////////////
ideguti 0:0805c5a1b328 489 //send to DAC
ideguti 0:0805c5a1b328 490 //////////////////////////////
ideguti 0:0805c5a1b328 491 //DCA1_A
ideguti 0:0805c5a1b328 492
ideguti 0:0805c5a1b328 493
ideguti 0:0805c5a1b328 494 }
ideguti 0:0805c5a1b328 495 #endif
ideguti 0:0805c5a1b328 496
ideguti 0:0805c5a1b328 497
ideguti 0:0805c5a1b328 498 void AutoAccel(double *tar_data, double *real_data, double *data, double roop_time, double accel){
ideguti 0:0805c5a1b328 499 static int max_range, state[4];
ideguti 0:0805c5a1b328 500 static double max[4], mini[4], range[4]={1,1,1,1};
ideguti 0:0805c5a1b328 501
ideguti 0:0805c5a1b328 502 for(int i=0; i<4; i++){
ideguti 0:0805c5a1b328 503 if(tar_data[i] >= real_data[i]){
ideguti 0:0805c5a1b328 504 if(state[i] == 0){
ideguti 0:0805c5a1b328 505 mini[i] = real_data[i];
ideguti 0:0805c5a1b328 506 range[i] = tar_data[i] - mini[i];
ideguti 0:0805c5a1b328 507 }
ideguti 0:0805c5a1b328 508 if(state[i] == 1){
ideguti 0:0805c5a1b328 509 range[i] = tar_data[i] - mini[i];
ideguti 0:0805c5a1b328 510 }
ideguti 0:0805c5a1b328 511 state[i] = 1;
ideguti 0:0805c5a1b328 512 }
ideguti 0:0805c5a1b328 513 else{
ideguti 0:0805c5a1b328 514 if(state[i] == 1){
ideguti 0:0805c5a1b328 515 max[i] = real_data[i];
ideguti 0:0805c5a1b328 516 range[i] = max[i] - tar_data[i];
ideguti 0:0805c5a1b328 517 }
ideguti 0:0805c5a1b328 518 if(state[i] == 0){
ideguti 0:0805c5a1b328 519 range[i] = max[i] - tar_data[i];
ideguti 0:0805c5a1b328 520 }
ideguti 0:0805c5a1b328 521 state[i] = 0;
ideguti 0:0805c5a1b328 522 }
ideguti 0:0805c5a1b328 523 }
ideguti 0:0805c5a1b328 524 for(int i=0; i<4; i++){
ideguti 0:0805c5a1b328 525 if(range[i] < 0) range[i] *= -1;
ideguti 0:0805c5a1b328 526 }
ideguti 0:0805c5a1b328 527 max_range = range[0];
ideguti 0:0805c5a1b328 528 for(int i=1; i<4; i++){
ideguti 0:0805c5a1b328 529 if(max_range < range[i]) max_range = range[i];
ideguti 0:0805c5a1b328 530 }
ideguti 0:0805c5a1b328 531
ideguti 0:0805c5a1b328 532 if(max_range == 0){
ideguti 0:0805c5a1b328 533 for(int i=0; i<4; i++){
ideguti 0:0805c5a1b328 534 range[i] = 0;
ideguti 0:0805c5a1b328 535 }
ideguti 0:0805c5a1b328 536 }
ideguti 0:0805c5a1b328 537 else {
ideguti 0:0805c5a1b328 538 for(int i=0; i<4; i++){
ideguti 0:0805c5a1b328 539 range[i] = range[i] / max_range;
ideguti 0:0805c5a1b328 540 }
ideguti 0:0805c5a1b328 541 }
ideguti 0:0805c5a1b328 542
ideguti 0:0805c5a1b328 543
ideguti 0:0805c5a1b328 544 for(int i=0; i<4; i++){
ideguti 0:0805c5a1b328 545 if(tar_data[i] - real_data[i] > 0){
ideguti 0:0805c5a1b328 546 if(real_data[i] > 0) data[i] += range[i]*accel*roop_time;
ideguti 0:0805c5a1b328 547 else data[i] += 1*range[i]*accel*roop_time;
ideguti 0:0805c5a1b328 548 }
ideguti 0:0805c5a1b328 549 if(tar_data[i] - real_data[i] < 0){
ideguti 0:0805c5a1b328 550 if(real_data[i] < 0) data[i] -= range[i]*accel*roop_time;
ideguti 0:0805c5a1b328 551 else data[i] -= 1*range[i]*accel*roop_time;
ideguti 0:0805c5a1b328 552 }
ideguti 0:0805c5a1b328 553 if(real_data[i] - tar_data[i] < range[i]*accel*roop_time && real_data[i] - tar_data[i] > -1*range[i]*accel*roop_time){
ideguti 0:0805c5a1b328 554 data[i] = tar_data[i];
ideguti 0:0805c5a1b328 555 }
ideguti 0:0805c5a1b328 556 }
ideguti 0:0805c5a1b328 557 }
ideguti 0:0805c5a1b328 558
ideguti 0:0805c5a1b328 559 void Saidaika(double *data)
ideguti 0:0805c5a1b328 560 {
ideguti 0:0805c5a1b328 561 double max = data[0];
ideguti 0:0805c5a1b328 562 for(int i=1; i<4; i++){
ideguti 0:0805c5a1b328 563 if(data[i] > max) max = data[i];
ideguti 0:0805c5a1b328 564 }
ideguti 0:0805c5a1b328 565 if(max > 4095){
ideguti 0:0805c5a1b328 566 for(int i=0; i<4; i++){
ideguti 0:0805c5a1b328 567 data[i] = data[i]*4095/max;
ideguti 0:0805c5a1b328 568 }
ideguti 0:0805c5a1b328 569 }
ideguti 0:0805c5a1b328 570 double mini = data[0];
ideguti 0:0805c5a1b328 571 for(int i=1; i<4; i++){
ideguti 0:0805c5a1b328 572 if(data[i] < mini) mini = data[i];
ideguti 0:0805c5a1b328 573 }
ideguti 0:0805c5a1b328 574 if(mini < -4095){
ideguti 0:0805c5a1b328 575 for(int i=0; i<4; i++){
ideguti 0:0805c5a1b328 576 data[i] = data[i]*4095/(-1*mini);
ideguti 0:0805c5a1b328 577 }
ideguti 0:0805c5a1b328 578 }
ideguti 0:0805c5a1b328 579 }
ideguti 0:0805c5a1b328 580
ideguti 0:0805c5a1b328 581 void Filter(double *data)
ideguti 0:0805c5a1b328 582 {
ideguti 0:0805c5a1b328 583 for(int i=0; i<4; i++){
ideguti 0:0805c5a1b328 584 if(data[i] < CUTOFF && data[i] > -1*CUTOFF) data[i] = 0;
ideguti 0:0805c5a1b328 585 }
ideguti 0:0805c5a1b328 586 }
ideguti 0:0805c5a1b328 587
ideguti 0:0805c5a1b328 588 void SendDAC(int *data)
ideguti 0:0805c5a1b328 589 {
ideguti 0:0805c5a1b328 590 dac_data=0;
ideguti 0:0805c5a1b328 591 if(data[0]>0)dac_data += data[0];
ideguti 0:0805c5a1b328 592 dac_data += (1<<ga);//gain1x
ideguti 0:0805c5a1b328 593 dac_data += (1<<shdn);//anarog on
ideguti 0:0805c5a1b328 594
ideguti 0:0805c5a1b328 595 ldac1=1;
ideguti 0:0805c5a1b328 596 ss1=0;
ideguti 0:0805c5a1b328 597 wait_us(10);
ideguti 0:0805c5a1b328 598 spi.write(dac_data);
ideguti 0:0805c5a1b328 599 wait_us(10);
ideguti 0:0805c5a1b328 600 ss1=1;
ideguti 0:0805c5a1b328 601 ldac1=0;
ideguti 0:0805c5a1b328 602 //DAC1_B
ideguti 0:0805c5a1b328 603 dac_data=0;
ideguti 0:0805c5a1b328 604 if(data[0]<0)dac_data -= data[0];
ideguti 0:0805c5a1b328 605 dac_data += (1<<ab);//OUTb
ideguti 0:0805c5a1b328 606 dac_data += (1<<ga);//gain1x
ideguti 0:0805c5a1b328 607 dac_data += (1<<shdn);//anarog on
ideguti 0:0805c5a1b328 608
ideguti 0:0805c5a1b328 609 ldac1=1;
ideguti 0:0805c5a1b328 610 ss1=0;
ideguti 0:0805c5a1b328 611 wait_us(10);
ideguti 0:0805c5a1b328 612 spi.write(dac_data);
ideguti 0:0805c5a1b328 613 wait_us(10);
ideguti 0:0805c5a1b328 614 ss1=1;
ideguti 0:0805c5a1b328 615 ldac1=0;
ideguti 0:0805c5a1b328 616 //DCA2_A
ideguti 0:0805c5a1b328 617 dac_data=0;
ideguti 0:0805c5a1b328 618 if(data[1]>0)dac_data += data[1];
ideguti 0:0805c5a1b328 619 dac_data += (1<<ga);//gain1x
ideguti 0:0805c5a1b328 620 dac_data += (1<<shdn);//anarog on
ideguti 0:0805c5a1b328 621
ideguti 0:0805c5a1b328 622 ldac2=1;
ideguti 0:0805c5a1b328 623 ss2=0;
ideguti 0:0805c5a1b328 624 wait_us(10);
ideguti 0:0805c5a1b328 625 spi.write(dac_data);
ideguti 0:0805c5a1b328 626 wait_us(10);
ideguti 0:0805c5a1b328 627 ss2=1;
ideguti 0:0805c5a1b328 628 ldac2=0;
ideguti 0:0805c5a1b328 629 //DAC2_B
ideguti 0:0805c5a1b328 630 dac_data=0;
ideguti 0:0805c5a1b328 631 if(data[1]<0)dac_data -= data[1];
ideguti 0:0805c5a1b328 632 dac_data += (1<<ab);//OUTb
ideguti 0:0805c5a1b328 633 dac_data += (1<<ga);//gain1x
ideguti 0:0805c5a1b328 634 dac_data += (1<<shdn);//anarog on
ideguti 0:0805c5a1b328 635
ideguti 0:0805c5a1b328 636 ldac2=1;
ideguti 0:0805c5a1b328 637 ss2=0;
ideguti 0:0805c5a1b328 638 wait_us(10);
ideguti 0:0805c5a1b328 639 spi.write(dac_data);
ideguti 0:0805c5a1b328 640 wait_us(10);
ideguti 0:0805c5a1b328 641 ss2=1;
ideguti 0:0805c5a1b328 642 ldac2=0;
ideguti 0:0805c5a1b328 643 //DCA3_A
ideguti 0:0805c5a1b328 644 dac_data=0;
ideguti 0:0805c5a1b328 645 if(data[2]>0)dac_data += data[2];
ideguti 0:0805c5a1b328 646 dac_data += (1<<ga);//gain1x
ideguti 0:0805c5a1b328 647 dac_data += (1<<shdn);//anarog on
ideguti 0:0805c5a1b328 648
ideguti 0:0805c5a1b328 649 ldac3=1;
ideguti 0:0805c5a1b328 650 ss3=0;
ideguti 0:0805c5a1b328 651 wait_us(10);
ideguti 0:0805c5a1b328 652 spi.write(dac_data);
ideguti 0:0805c5a1b328 653 wait_us(10);
ideguti 0:0805c5a1b328 654 ss3=1;
ideguti 0:0805c5a1b328 655 ldac3=0;
ideguti 0:0805c5a1b328 656 //DAC3_B
ideguti 0:0805c5a1b328 657 dac_data=0;
ideguti 0:0805c5a1b328 658 if(data[2]<0)dac_data -= data[2];
ideguti 0:0805c5a1b328 659 dac_data += (1<<ab);//OUTb
ideguti 0:0805c5a1b328 660 dac_data += (1<<ga);//gain1x
ideguti 0:0805c5a1b328 661 dac_data += (1<<shdn);//anarog on
ideguti 0:0805c5a1b328 662
ideguti 0:0805c5a1b328 663 ldac3=1;
ideguti 0:0805c5a1b328 664 ss3=0;
ideguti 0:0805c5a1b328 665 wait_us(10);
ideguti 0:0805c5a1b328 666 spi.write(dac_data);
ideguti 0:0805c5a1b328 667 wait_us(10);
ideguti 0:0805c5a1b328 668 ss3=1;
ideguti 0:0805c5a1b328 669 ldac3=0;
ideguti 0:0805c5a1b328 670 //DCA4_A
ideguti 0:0805c5a1b328 671 dac_data=0;
ideguti 0:0805c5a1b328 672 if(data[3]>0)dac_data += data[3];
ideguti 0:0805c5a1b328 673 dac_data += (1<<ga);//gain1x
ideguti 0:0805c5a1b328 674 dac_data += (1<<shdn);//anarog on
ideguti 0:0805c5a1b328 675
ideguti 0:0805c5a1b328 676 ldac4=1;
ideguti 0:0805c5a1b328 677 ss4=0;
ideguti 0:0805c5a1b328 678 wait_us(10);
ideguti 0:0805c5a1b328 679 spi.write(dac_data);
ideguti 0:0805c5a1b328 680 wait_us(10);
ideguti 0:0805c5a1b328 681 ss4=1;
ideguti 0:0805c5a1b328 682 ldac4=0;
ideguti 0:0805c5a1b328 683
ideguti 0:0805c5a1b328 684 //DAC4_B
ideguti 0:0805c5a1b328 685 dac_data=0;
ideguti 0:0805c5a1b328 686 if(data[3]<0)dac_data -= data[3];
ideguti 0:0805c5a1b328 687 dac_data += (1<<ab);//OUTb
ideguti 0:0805c5a1b328 688 dac_data += (1<<ga);//gain1x
ideguti 0:0805c5a1b328 689 dac_data += (1<<shdn);//anarog on
ideguti 0:0805c5a1b328 690
ideguti 0:0805c5a1b328 691 ldac4=1;
ideguti 0:0805c5a1b328 692 ss4=0;
ideguti 0:0805c5a1b328 693 wait_us(10);
ideguti 0:0805c5a1b328 694 spi.write(dac_data);
ideguti 0:0805c5a1b328 695 wait_us(10);
ideguti 0:0805c5a1b328 696 ss4=1;
ideguti 0:0805c5a1b328 697 ldac4=0;
ideguti 0:0805c5a1b328 698 //END of send to DAC
ideguti 0:0805c5a1b328 699 }
ideguti 0:0805c5a1b328 700
ideguti 0:0805c5a1b328 701
ideguti 0:0805c5a1b328 702 int TypeDivider(void *pType, char byte[]){
ideguti 0:0805c5a1b328 703 short size = sizeof(pType);
ideguti 0:0805c5a1b328 704 if(size > sizeof(byte)){
ideguti 0:0805c5a1b328 705 return -1; //Error
ideguti 0:0805c5a1b328 706 }
ideguti 0:0805c5a1b328 707 for(short i=0;i<size;i++){
ideguti 0:0805c5a1b328 708 byte[i] = *(( (char*)pType ) + i);
ideguti 0:0805c5a1b328 709 }
ideguti 0:0805c5a1b328 710 return 0;
ideguti 0:0805c5a1b328 711 }
ideguti 0:0805c5a1b328 712
ideguti 0:0805c5a1b328 713 int TypeConstructor(void *pType, char byte[]){
ideguti 0:0805c5a1b328 714 short size = sizeof(pType);
ideguti 0:0805c5a1b328 715 for(short i=0;i<size;i++){
ideguti 0:0805c5a1b328 716 *(( (char*)pType ) + i) = byte[i];
ideguti 0:0805c5a1b328 717 }
ideguti 0:0805c5a1b328 718 return 0;
ideguti 0:0805c5a1b328 719 }
ideguti 0:0805c5a1b328 720
ideguti 0:0805c5a1b328 721 double sai1(double x[],double y[])
ideguti 0:0805c5a1b328 722 {
ideguti 0:0805c5a1b328 723
ideguti 0:0805c5a1b328 724 int i;
ideguti 0:0805c5a1b328 725 double a0,a1,p,q;
ideguti 0:0805c5a1b328 726 double A00,A01,A02,A11,A12;
ideguti 0:0805c5a1b328 727
ideguti 0:0805c5a1b328 728
ideguti 0:0805c5a1b328 729
ideguti 0:0805c5a1b328 730 A00=A01=A02=A11=A12=0.0;
ideguti 0:0805c5a1b328 731
ideguti 0:0805c5a1b328 732 for (i=0;i<5;i++) {
ideguti 0:0805c5a1b328 733 A00+=1.0;
ideguti 0:0805c5a1b328 734 A01+=x[i];
ideguti 0:0805c5a1b328 735 A02+=y[i];
ideguti 0:0805c5a1b328 736 A11+=x[i]*x[i];
ideguti 0:0805c5a1b328 737 A12+=x[i]*y[i];
ideguti 0:0805c5a1b328 738 }
ideguti 0:0805c5a1b328 739
ideguti 0:0805c5a1b328 740 /*１次式の係数の計算*/
ideguti 0:0805c5a1b328 741 a0=(A02*A11-A01*A12)/(A00*A11-A01*A01);
ideguti 0:0805c5a1b328 742 a1=(A00*A12-A01*A02)/(A00*A11-A01*A01);
ideguti 0:0805c5a1b328 743
ideguti 0:0805c5a1b328 744
ideguti 0:0805c5a1b328 745
ideguti 0:0805c5a1b328 746 /*gnuplotでグラフ表示のために
ideguti 0:0805c5a1b328 747 計算で得られた１次式でデータ近辺のグラフデータを保存*/
ideguti 0:0805c5a1b328 748 return a1;
ideguti 0:0805c5a1b328 749 }