匡尚 青柳 / Mbed 2 deprecated absorber

あの装置用

Dependencies:   ADXL345_I2C QEI SDFileSystem mbed

main.cpp@8:9059f95fc498, 2017-03-24 (annotated)

Committer:
ojityan
Date:
Fri Mar 24 07:03:37 2017 +0000
Revision:
8:9059f95fc498
Parent:
7:0e3fac3c6a6a 
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojityan 0:86efb6256f31 1 #include "mbed.h"
ojityan 0:86efb6256f31 2 #include "ADXL345_I2C.h"
ojityan 0:86efb6256f31 3 #include <stdio.h>
ojityan 0:86efb6256f31 4 #include <stdlib.h>
ojityan 0:86efb6256f31 5
ojityan 0:86efb6256f31 6 void savedata(void);
ojityan 0:86efb6256f31 7 void flipB(void);
ojityan 1:a20656b5bfe1 8 void ADXL_config(void);
ojityan 1:a20656b5bfe1 9 void read_current(double *C);
ojityan 2:49d3b3eebc2e 10 void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F);
ojityan 1:a20656b5bfe1 11 void makefile(void);
ojityan 1:a20656b5bfe1 12 void weight_init(void);
ojityan 1:a20656b5bfe1 13 void INA226config(void);
ojityan 0:86efb6256f31 14
ojityan 0:86efb6256f31 15 DigitalOut dir(p25);
ojityan 0:86efb6256f31 16 PwmOut duty(p24);
ojityan 0:86efb6256f31 17 I2C i2c(p28, p27);
ojityan 0:86efb6256f31 18
ojityan 0:86efb6256f31 19 ADXL345_I2C accelerometer(p9, p10);
ojityan 0:86efb6256f31 20 Serial debug(USBTX,USBRX);
ojityan 0:86efb6256f31 21 //QEI wheel (p21, p22, p23, 1000);
ojityan 0:86efb6256f31 22
ojityan 0:86efb6256f31 23 DigitalOut myled1(LED1);
ojityan 0:86efb6256f31 24 DigitalOut myled2(LED2);
ojityan 0:86efb6256f31 25 DigitalOut myled3(LED3);
ojityan 0:86efb6256f31 26 DigitalOut myled4(LED4);
ojityan 0:86efb6256f31 27 DigitalIn sensorA(p17);
ojityan 0:86efb6256f31 28 DigitalIn sensorB(p18);
ojityan 0:86efb6256f31 29 DigitalIn sensorC(p19);
ojityan 0:86efb6256f31 30 DigitalIn sensorD(p20);
ojityan 0:86efb6256f31 31 InterruptIn sensorB_(p18);
ojityan 0:86efb6256f31 32
ojityan 0:86efb6256f31 33 int flag = 0;
ojityan 0:86efb6256f31 34 int flipB_ = 0;
ojityan 0:86efb6256f31 35 Ticker save;
ojityan 0:86efb6256f31 36 FILE *fp;
ojityan 0:86efb6256f31 37 Timer t;
ojityan 4:1ec5063fb20e 38 int intrrupt_cnt = 0;
ojityan 0:86efb6256f31 39
ojityan 0:86efb6256f31 40 static char i2c_addr = 0x80;
ojityan 0:86efb6256f31 41
ojityan 0:86efb6256f31 42
ojityan 0:86efb6256f31 43
ojityan 0:86efb6256f31 44 int main()
ojityan 0:86efb6256f31 45 {
ojityan 2:49d3b3eebc2e 46 char buffer[100][150];
ojityan 0:86efb6256f31 47 int i,j;
ojityan 0:86efb6256f31 48 double C;
ojityan 0:86efb6256f31 49 float duty_output;
ojityan 0:86efb6256f31 50 int loop_break;
ojityan 1:a20656b5bfe1 51 double acce_device;
ojityan 1:a20656b5bfe1 52 double acce_output;
ojityan 0:86efb6256f31 53 int readings[3] = {0, 0, 0};
ojityan 0:86efb6256f31 54 double v;
ojityan 2:49d3b3eebc2e 55 double F_output;
ojityan 0:86efb6256f31 56
ojityan 7:0e3fac3c6a6a 57 myled1 = 1; //起動の確認
ojityan 1:a20656b5bfe1 58 ADXL_config();
ojityan 1:a20656b5bfe1 59 myled2 = 1;
ojityan 1:a20656b5bfe1 60 // makefile();
ojityan 0:86efb6256f31 61 myled3 = 1;
ojityan 1:a20656b5bfe1 62 INA226config();
ojityan 0:86efb6256f31 63 myled4 = 1;
ojityan 0:86efb6256f31 64 wait(3.0);
ojityan 0:86efb6256f31 65 myled1 = 0;
ojityan 0:86efb6256f31 66 myled2 = 0;
ojityan 0:86efb6256f31 67 myled3 = 0;
ojityan 0:86efb6256f31 68 myled4 = 0;
ojityan 1:a20656b5bfe1 69
ojityan 1:a20656b5bfe1 70
ojityan 7:0e3fac3c6a6a 71 //---PWNの周期を設定---
ojityan 5:473edb6558fa 72 duty_output = 0.6;
ojityan 0:86efb6256f31 73 duty.period(0.00005);
ojityan 0:86efb6256f31 74 // NVIC_SetPriority(0, 1);
ojityan 0:86efb6256f31 75
ojityan 0:86efb6256f31 76
ojityan 0:86efb6256f31 77 /////////////////////
ojityan 7:0e3fac3c6a6a 78 //---制御開始---
ojityan 0:86efb6256f31 79 /////////////////////
ojityan 0:86efb6256f31 80 while(1) {
ojityan 0:86efb6256f31 81 t.reset();
ojityan 0:86efb6256f31 82 i = 0;
ojityan 0:86efb6256f31 83 loop_break = 0;
ojityan 0:86efb6256f31 84 debug.printf("writing OK!\n");
ojityan 2:49d3b3eebc2e 85
ojityan 7:0e3fac3c6a6a 86 weight_init(); //慣性力発生用おもりを初期位置に移動させる
ojityan 0:86efb6256f31 87
ojityan 7:0e3fac3c6a6a 88 //---加速度を感知したら移動開始---
ojityan 0:86efb6256f31 89 flipB_ = 0;
ojityan 0:86efb6256f31 90 dir = 1;
ojityan 1:a20656b5bfe1 91
ojityan 0:86efb6256f31 92 while (1) {
ojityan 0:86efb6256f31 93 accelerometer.getOutput(readings);
ojityan 7:0e3fac3c6a6a 94 if( abs((int16_t)readings[0]) > 25) {
ojityan 0:86efb6256f31 95 myled3 = 1;
ojityan 0:86efb6256f31 96 break;
ojityan 0:86efb6256f31 97 }
ojityan 0:86efb6256f31 98 }
ojityan 8:9059f95fc498 99 save.attach(&savedata, 0.01 ); //割り込み開始
ojityan 0:86efb6256f31 100 t.start();
ojityan 7:0e3fac3c6a6a 101
ojityan 7:0e3fac3c6a6a 102 duty.write(duty_output);
ojityan 1:a20656b5bfe1 103
ojityan 1:a20656b5bfe1 104
ojityan 8:9059f95fc498 105 while(t.read() <= 0.80) { //内部タイマーが0.80秒に達するまで制御
ojityan 0:86efb6256f31 106 if(flag) {
ojityan 0:86efb6256f31 107 flag = 0;
ojityan 2:49d3b3eebc2e 108 controller( &v, &duty_output, &acce_device, &acce_output, &F_output);
ojityan 1:a20656b5bfe1 109 read_current( &C);
ojityan 2:49d3b3eebc2e 110 if(1) {
ojityan 8:9059f95fc498 111 sprintf(buffer[i],"%f10,%f10,%f10,%f10,%f10,%f10",t.read(),C,acce_device,C,duty_output,F_output); //バッファにデータを溜めこみ終了時に吐き出す．
ojityan 2:49d3b3eebc2e 112 if( i > 299) {
ojityan 2:49d3b3eebc2e 113 loop_break = 1;
ojityan 2:49d3b3eebc2e 114 break;
ojityan 2:49d3b3eebc2e 115 }
ojityan 2:49d3b3eebc2e 116 i = i + 1;
ojityan 2:49d3b3eebc2e 117 }
ojityan 0:86efb6256f31 118
ojityan 0:86efb6256f31 119 }
ojityan 0:86efb6256f31 120 if(loop_break) {
ojityan 0:86efb6256f31 121 break;
ojityan 0:86efb6256f31 122 }
ojityan 0:86efb6256f31 123
ojityan 0:86efb6256f31 124 }
ojityan 7:0e3fac3c6a6a 125
ojityan 8:9059f95fc498 126 myled2 = 1; //0.80秒経過したらLED2を光らせる
ojityan 0:86efb6256f31 127 t.stop();
ojityan 2:49d3b3eebc2e 128 save.detach();
ojityan 0:86efb6256f31 129
ojityan 0:86efb6256f31 130
ojityan 0:86efb6256f31 131 duty = 0.0f;
ojityan 2:49d3b3eebc2e 132
ojityan 2:49d3b3eebc2e 133 myled1 = 1;
ojityan 0:86efb6256f31 134 for( j = 0; j < i; j++) {
ojityan 7:0e3fac3c6a6a 135 debug.printf("%s\n",buffer[j]);//シリアル通信でバッファにあるデータを相手に送信する
ojityan 0:86efb6256f31 136 }
ojityan 7:0e3fac3c6a6a 137
ojityan 0:86efb6256f31 138 while(1) {
ojityan 2:49d3b3eebc2e 139 myled1 = 1;
ojityan 0:86efb6256f31 140 wait(0.2);
ojityan 2:49d3b3eebc2e 141 myled1 = 0;
ojityan 0:86efb6256f31 142 wait(0.2);
ojityan 0:86efb6256f31 143 }
ojityan 0:86efb6256f31 144 }
ojityan 0:86efb6256f31 145 }
ojityan 0:86efb6256f31 146
ojityan 0:86efb6256f31 147
ojityan 0:86efb6256f31 148
ojityan 0:86efb6256f31 149
ojityan 7:0e3fac3c6a6a 150 void savedata(void)//割り込み時の処理
ojityan 0:86efb6256f31 151 {
ojityan 0:86efb6256f31 152 flag = 1;
ojityan 4:1ec5063fb20e 153 intrrupt_cnt++;
ojityan 2:49d3b3eebc2e 154
ojityan 0:86efb6256f31 155 }
ojityan 0:86efb6256f31 156
ojityan 8:9059f95fc498 157
ojityan 8:9059f95fc498 158 void ADXL_config(void)//ADXL345の初期設定
ojityan 0:86efb6256f31 159 {
ojityan 8:9059f95fc498 160 //初期設定開始
ojityan 1:a20656b5bfe1 161 accelerometer.setPowerControl(0x00);
ojityan 1:a20656b5bfe1 162
ojityan 8:9059f95fc498 163 //精度
ojityan 1:a20656b5bfe1 164 accelerometer.setDataFormatControl(0x0B);
ojityan 1:a20656b5bfe1 165
ojityan 8:9059f95fc498 166 //3.2kHz
ojityan 1:a20656b5bfe1 167 accelerometer.setDataRate(ADXL345_3200HZ);
ojityan 1:a20656b5bfe1 168
ojityan 8:9059f95fc498 169 //計測開始
ojityan 1:a20656b5bfe1 170 accelerometer.setPowerControl(0x08);
ojityan 1:a20656b5bfe1 171
ojityan 1:a20656b5bfe1 172 accelerometer.setOffset (0, 0);
ojityan 1:a20656b5bfe1 173
ojityan 1:a20656b5bfe1 174 }
ojityan 1:a20656b5bfe1 175
ojityan 8:9059f95fc498 176 void INA226config(void) //INA226の設定
ojityan 1:a20656b5bfe1 177 {
ojityan 1:a20656b5bfe1 178
ojityan 1:a20656b5bfe1 179 char p_addr = 0x00;
ojityan 1:a20656b5bfe1 180 char p_addr_i = 0x04;
ojityan 1:a20656b5bfe1 181 short Calibration_set = 0x0A00;
ojityan 1:a20656b5bfe1 182 char write_item[3];
ojityan 1:a20656b5bfe1 183 //---ammeter setting---
ojityan 1:a20656b5bfe1 184 i2c.start();
ojityan 1:a20656b5bfe1 185 if(i2c.write(i2c_addr | 0,&p_addr,sizeof(p_addr)) == 0) {
ojityan 8:9059f95fc498 186 debug.printf("ERROR_write\n");
ojityan 1:a20656b5bfe1 187 }
ojityan 1:a20656b5bfe1 188 debug.printf("OK!\n");
ojityan 1:a20656b5bfe1 189
ojityan 1:a20656b5bfe1 190 //rawWrite(0x05,0x0A00);
ojityan 1:a20656b5bfe1 191
ojityan 1:a20656b5bfe1 192 short Config_set = 0x444F;//0x4A4F;//0x4E97;//0x4C97;
ojityan 1:a20656b5bfe1 193 write_item[0] = 0x00;
ojityan 1:a20656b5bfe1 194 write_item[1] = static_cast<char>((Config_set >> 8) & 0x00ff);
ojityan 1:a20656b5bfe1 195 write_item[2] = static_cast<char>(Config_set & 0x00ff);
ojityan 1:a20656b5bfe1 196 if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
ojityan 1:a20656b5bfe1 197 debug.printf("OK!!\n");
ojityan 1:a20656b5bfe1 198 } else {
ojityan 1:a20656b5bfe1 199 debug.printf("OH!!\n");
ojityan 1:a20656b5bfe1 200 }
ojityan 1:a20656b5bfe1 201
ojityan 1:a20656b5bfe1 202
ojityan 1:a20656b5bfe1 203
ojityan 1:a20656b5bfe1 204 write_item[0] = 0x05;
ojityan 1:a20656b5bfe1 205 write_item[1] = static_cast<char>((Calibration_set >> 8) & 0x00ff);
ojityan 1:a20656b5bfe1 206 write_item[2] = static_cast<char>(Calibration_set & 0x00ff);
ojityan 1:a20656b5bfe1 207 if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
ojityan 1:a20656b5bfe1 208 debug.printf("OK!!\n");
ojityan 1:a20656b5bfe1 209 } else {
ojityan 1:a20656b5bfe1 210 debug.printf("OH!!\n");
ojityan 1:a20656b5bfe1 211 }
ojityan 1:a20656b5bfe1 212 }
ojityan 1:a20656b5bfe1 213
ojityan 8:9059f95fc498 214 void weight_init(void) //慣性力発生用のおもりの位置の初期化
ojityan 1:a20656b5bfe1 215 {
ojityan 1:a20656b5bfe1 216 //---position reset---
ojityan 1:a20656b5bfe1 217 dir = 0;
ojityan 1:a20656b5bfe1 218 duty = 0.50f;
ojityan 7:0e3fac3c6a6a 219 while(!sensorB);
ojityan 1:a20656b5bfe1 220 duty = 0.0f;
ojityan 1:a20656b5bfe1 221 wait(2.0);
ojityan 7:0e3fac3c6a6a 222 myled1 = 1;
ojityan 1:a20656b5bfe1 223 }
ojityan 1:a20656b5bfe1 224
ojityan 8:9059f95fc498 225 void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F)//制御部
ojityan 1:a20656b5bfe1 226 {
ojityan 2:49d3b3eebc2e 227
ojityan 1:a20656b5bfe1 228 double ka = 100;
ojityan 1:a20656b5bfe1 229 double kv = 100;
ojityan 2:49d3b3eebc2e 230
ojityan 1:a20656b5bfe1 231 int readings[3] = {0,0,0};
ojityan 2:49d3b3eebc2e 232 // static double F;
ojityan 2:49d3b3eebc2e 233 static double F_1;
ojityan 2:49d3b3eebc2e 234 static double F_2;
ojityan 2:49d3b3eebc2e 235 static double F_3;
ojityan 7:0e3fac3c6a6a 236 static int intrrupt_checker = 1;
ojityan 1:a20656b5bfe1 237
ojityan 1:a20656b5bfe1 238
ojityan 8:9059f95fc498 239 //---加速度を読み取り変換する---
ojityan 1:a20656b5bfe1 240 accelerometer.getOutput(readings);
ojityan 1:a20656b5bfe1 241 *acce_device = (int16_t)readings[0] * 0.0383;
ojityan 1:a20656b5bfe1 242 *v = *v + *acce_device * 0.01;
ojityan 2:49d3b3eebc2e 243 if( *acce_device < 0) {
ojityan 1:a20656b5bfe1 244 if(abs(*v) > 0) {
ojityan 8:9059f95fc498 245 //---必要な力の算出（今回は不使用）---
ojityan 1:a20656b5bfe1 246 F_1 = -1 * ( ka * *acce_device) + (kv * *v);
ojityan 2:49d3b3eebc2e 247 *F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3;
ojityan 2:49d3b3eebc2e 248 *acce_output = *F / 8.5;
ojityan 3:de6579180af2 249 if(*duty_output > 1.0) {
ojityan 3:de6579180af2 250 *duty_output = 1.0;
ojityan 1:a20656b5bfe1 251 myled4 = 1;
ojityan 1:a20656b5bfe1 252 }
ojityan 8:9059f95fc498 253 //---加速度を検知したら慣性力発生用おもりを加速させる---
ojityan 7:0e3fac3c6a6a 254 if(intrrupt_checker <= intrrupt_cnt){
ojityan 7:0e3fac3c6a6a 255 *duty_output = *duty_output + 0.04;
ojityan 7:0e3fac3c6a6a 256 intrrupt_checker = intrrupt_cnt + 1;
ojityan 4:1ec5063fb20e 257 }
ojityan 4:1ec5063fb20e 258
ojityan 4:1ec5063fb20e 259
ojityan 4:1ec5063fb20e 260 duty.write(*duty_output);
ojityan 8:9059f95fc498 261
ojityan 1:a20656b5bfe1 262 F_3 = F_2;
ojityan 2:49d3b3eebc2e 263 F_2 = *F;
ojityan 1:a20656b5bfe1 264 }
ojityan 1:a20656b5bfe1 265 }
ojityan 1:a20656b5bfe1 266 }
ojityan 1:a20656b5bfe1 267
ojityan 8:9059f95fc498 268 void read_current(double *C) //電流量を測る
ojityan 1:a20656b5bfe1 269 {
ojityan 1:a20656b5bfe1 270 char I[2];
ojityan 1:a20656b5bfe1 271 unsigned short read_item;
ojityan 1:a20656b5bfe1 272 char p_addr = 0x00;
ojityan 1:a20656b5bfe1 273 char p_addr_i = 0x04;
ojityan 1:a20656b5bfe1 274 //---reading current---
ojityan 1:a20656b5bfe1 275 while(i2c.write(i2c_addr | 0,&p_addr_i,sizeof(p_addr_i)) != 0);
ojityan 1:a20656b5bfe1 276 while(i2c.read(i2c_addr | 0x01,I,sizeof(I)) != 0);
ojityan 1:a20656b5bfe1 277 read_item = static_cast<unsigned short>(I[0]);
ojityan 1:a20656b5bfe1 278 read_item = (read_item << 8) | static_cast<unsigned short>(I[1]);
ojityan 1:a20656b5bfe1 279
ojityan 1:a20656b5bfe1 280 char *s_p = reinterpret_cast<char *>(&read_item);
ojityan 1:a20656b5bfe1 281 short d_s;
ojityan 1:a20656b5bfe1 282 char *d_p = reinterpret_cast<char *>(&d_s);
ojityan 1:a20656b5bfe1 283 *(d_p + 0) = *(s_p + 0);
ojityan 1:a20656b5bfe1 284 *(d_p + 1) = *(s_p + 1);
ojityan 1:a20656b5bfe1 285 *C = static_cast<double>(d_s) /* * 1.25 */;
ojityan 1:a20656b5bfe1 286 }