main.cpp

00001 #include "mbed.h"
00002 #include "ADXL345_I2C.h"
00003 #include <stdio.h>
00004 #include <stdlib.h>
00005 
00006 void savedata(void);
00007 void flipB(void);
00008 void ADXL_config(void);
00009 void read_current(double *C);
00010 void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F);
00011 void makefile(void);
00012 void weight_init(void);
00013 void INA226config(void);
00014 
00015 DigitalOut dir(p25);
00016 PwmOut duty(p24);
00017 I2C i2c(p28, p27);
00018 
00019 ADXL345_I2C accelerometer(p9, p10);
00020 Serial debug(USBTX,USBRX);
00021 //QEI wheel (p21, p22, p23, 1000);
00022 
00023 DigitalOut myled1(LED1);
00024 DigitalOut myled2(LED2);
00025 DigitalOut myled3(LED3);
00026 DigitalOut myled4(LED4);
00027 DigitalIn  sensorA(p17);
00028 DigitalIn  sensorB(p18);
00029 DigitalIn  sensorC(p19);
00030 DigitalIn  sensorD(p20);
00031 InterruptIn sensorB_(p18);
00032 
00033 int flag = 0;
00034 int flipB_ = 0;
00035 Ticker save;
00036 FILE *fp;
00037 Timer t;
00038 int intrrupt_cnt = 0;
00039 
00040 static char i2c_addr = 0x80;
00041 
00042 
00043 
00044 int main()
00045 {
00046     char buffer[100][150];
00047     int i,j;
00048     double C;
00049     float duty_output;
00050     int loop_break;
00051     double acce_device;
00052     double acce_output;
00053     int readings[3] = {0, 0, 0};
00054     double v;
00055     double F_output;
00056 
00057     myled1 = 1; //起動の確認
00058     ADXL_config();
00059     myled2 = 1;
00060 //    makefile();
00061     myled3 = 1;
00062     INA226config();
00063     myled4 = 1;
00064     wait(3.0);
00065     myled1 = 0;
00066     myled2 = 0;
00067     myled3 = 0;
00068     myled4 = 0;
00069 
00070 
00071 //---PWNの周期を設定---
00072     duty_output = 0.6;
00073     duty.period(0.00005);
00074 //    NVIC_SetPriority(0, 1);
00075 
00076 
00077 /////////////////////
00078 //---制御開始---
00079 /////////////////////
00080     while(1) {
00081         t.reset();
00082         i = 0;
00083         loop_break = 0;
00084         debug.printf("writing OK!\n");
00085 
00086         weight_init(); //慣性力発生用おもりを初期位置に移動させる
00087 
00088 //---加速度を感知したら移動開始---
00089         flipB_ = 0;
00090         dir = 1;
00091 
00092         while (1) {
00093             accelerometer.getOutput(readings);
00094             if( abs((int16_t)readings[0]) > 25) {
00095                 myled3 = 1;
00096                 break;
00097             }
00098         }
00099         save.attach(&savedata, 0.01 ); //割り込み開始
00100         t.start();
00101         
00102         duty.write(duty_output);
00103 
00104 
00105         while(t.read() <= 0.80) { //内部タイマーが0.80秒に達するまで制御
00106             if(flag) {
00107                 flag = 0;
00108                 controller( &v, &duty_output, &acce_device, &acce_output, &F_output);
00109                 read_current( &C);
00110                 if(1) {
00111                     sprintf(buffer[i],"%f10,%f10,%f10,%f10,%f10,%f10",t.read(),C,acce_device,C,duty_output,F_output); //バッファにデータを溜めこみ終了時に吐き出す．
00112                     if( i > 299) {
00113                         loop_break = 1;
00114                         break;
00115                     }
00116                     i = i + 1;
00117                 }
00118 
00119             }
00120             if(loop_break) {
00121                 break;
00122             }
00123 
00124         }
00125         
00126         myled2 = 1; //0.80秒経過したらLED2を光らせる
00127         t.stop();
00128         save.detach();
00129 
00130 
00131         duty = 0.0f;
00132 
00133         myled1 = 1;
00134         for( j = 0; j < i; j++) {
00135             debug.printf("%s\n",buffer[j]);//シリアル通信でバッファにあるデータを相手に送信する
00136         }
00137         
00138         while(1) {
00139             myled1 = 1;
00140             wait(0.2);
00141             myled1 = 0;
00142             wait(0.2);
00143         }
00144     }
00145 }
00146 
00147 
00148 
00149 
00150 void savedata(void)//割り込み時の処理
00151 {
00152     flag = 1;
00153     intrrupt_cnt++;
00154 
00155 }
00156 
00157 
00158 void ADXL_config(void)//ADXL345の初期設定
00159 {
00160     //初期設定開始
00161     accelerometer.setPowerControl(0x00);
00162 
00163     //精度
00164     accelerometer.setDataFormatControl(0x0B);
00165 
00166     //3.2kHz
00167     accelerometer.setDataRate(ADXL345_3200HZ);
00168 
00169     //計測開始
00170     accelerometer.setPowerControl(0x08);
00171 
00172     accelerometer.setOffset (0, 0);
00173 
00174 }
00175 
00176 void INA226config(void) //INA226の設定
00177 {
00178 
00179     char p_addr = 0x00;
00180     char p_addr_i = 0x04;
00181     short Calibration_set = 0x0A00;
00182     char write_item[3];
00183 //---ammeter setting---
00184     i2c.start();
00185     if(i2c.write(i2c_addr | 0,&p_addr,sizeof(p_addr)) == 0) {
00186         debug.printf("ERROR_write\n");
00187     }
00188     debug.printf("OK!\n");
00189 
00190     //rawWrite(0x05,0x0A00);
00191 
00192     short Config_set = 0x444F;//0x4A4F;//0x4E97;//0x4C97;
00193     write_item[0] = 0x00;
00194     write_item[1] = static_cast<char>((Config_set >> 8) & 0x00ff);
00195     write_item[2] = static_cast<char>(Config_set & 0x00ff);
00196     if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
00197         debug.printf("OK!!\n");
00198     } else {
00199         debug.printf("OH!!\n");
00200     }
00201 
00202 
00203 
00204     write_item[0] = 0x05;
00205     write_item[1] = static_cast<char>((Calibration_set >> 8) & 0x00ff);
00206     write_item[2] = static_cast<char>(Calibration_set & 0x00ff);
00207     if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
00208         debug.printf("OK!!\n");
00209     } else {
00210         debug.printf("OH!!\n");
00211     }
00212 }
00213 
00214 void weight_init(void) //慣性力発生用のおもりの位置の初期化
00215 {
00216 //---position reset---
00217     dir = 0;
00218     duty = 0.50f;
00219     while(!sensorB);
00220     duty = 0.0f;
00221     wait(2.0);
00222     myled1 = 1;
00223 }
00224 
00225 void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F)//制御部
00226 {
00227 
00228     double ka = 100;
00229     double kv = 100;
00230 
00231     int readings[3] = {0,0,0};
00232 //    static double F;
00233     static double F_1;
00234     static double F_2;
00235     static double F_3;
00236     static int intrrupt_checker = 1;
00237 
00238 
00239 //---加速度を読み取り変換する---
00240     accelerometer.getOutput(readings);
00241     *acce_device = (int16_t)readings[0] * 0.0383;
00242     *v = *v + *acce_device * 0.01;
00243     if( *acce_device < 0) {
00244         if(abs(*v) > 0) {
00245 //---必要な力の算出（今回は不使用）---
00246             F_1 = -1 * ( ka * *acce_device) + (kv * *v);
00247             *F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3;
00248             *acce_output = *F / 8.5;
00249             if(*duty_output > 1.0) {
00250                 *duty_output = 1.0;
00251                 myled4 = 1;
00252             }
00253 //---加速度を検知したら慣性力発生用おもりを加速させる---
00254             if(intrrupt_checker <= intrrupt_cnt){
00255                 *duty_output = *duty_output + 0.04;
00256                 intrrupt_checker = intrrupt_cnt + 1;
00257             }
00258             
00259             
00260             duty.write(*duty_output);
00261 
00262             F_3 = F_2;
00263             F_2 = *F;
00264         }
00265     }
00266 }
00267 
00268 void read_current(double *C) //電流量を測る
00269 {
00270     char I[2];
00271     unsigned short read_item;
00272     char p_addr = 0x00;
00273     char p_addr_i = 0x04;
00274 //---reading current---
00275     while(i2c.write(i2c_addr | 0,&p_addr_i,sizeof(p_addr_i)) != 0);
00276     while(i2c.read(i2c_addr | 0x01,I,sizeof(I)) != 0);
00277     read_item = static_cast<unsigned short>(I[0]);
00278     read_item = (read_item << 8) | static_cast<unsigned short>(I[1]);
00279 
00280     char *s_p = reinterpret_cast<char *>(&read_item);
00281     short d_s;
00282     char *d_p = reinterpret_cast<char *>(&d_s);
00283     *(d_p + 0) = *(s_p + 0);
00284     *(d_p + 1) = *(s_p + 1);
00285     *C = static_cast<double>(d_s) /* * 1.25 */;
00286 }