![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test
Dependencies: z00_NSYS_CAR mbed SDFileSystem
Diff: main1220.cpp
- Revision:
- 1:32dfdcc9490f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main1220.cpp Wed Dec 23 08:49:47 2020 +0000 @@ -0,0 +1,277 @@ +/*========================================= +Title:4axis + +Revision:1.0.0 + +Editor:Dyson + +For GR-PEACH +Compile using Mbed Online Compiler (revision:130) + +reminder: +Last Modified:2018/09/20 +==========================================*/ + +//=====CONSTANT VARIBLES======== +#define INDICATOR 1//Front:0 Rear:1 +#define BUFF_DEPTH 128 +#define BUFF_COUNT 3 + +//======GLOBAL VARIBLES========= +char adcsel = 0; + +int sum = 7; + +int kill = 0; +int KILL_COUNT; +float t_pulse = 0; + +int buff_depth = 0; +int buff_index = 0; +int cur_buff=0; + +int flg =0; +typedef struct Logdata_t{ + int acc[4]; + float t_acc; + float m_t_pulse; +}Logdata; +Logdata data[BUFF_COUNT][BUFF_DEPTH]; + +bool write_ok[BUFF_COUNT]; + +int rch[3]; + +//=========SYSTEM============== +#include "mbed.h" +#define _Enable 0 +#define _Disable 1 +Serial gps(P8_13,P8_11); +Serial pc(USBTX,USBRX); +InterruptIn PPS(P2_15); +Timer utime; +Ticker adc_ticker; +//Ticker gps_ticker; +SPI adc(P10_14, P10_15, P10_12); +DigitalOut cs_adc(P8_15); +DigitalOut myled_r(LED1); +DigitalOut myled_g(LED2); +DigitalOut myled_b(LED3); + + +void adc_init(void){ + pc.printf("."); + cs_adc = _Enable;// ADC chip select on + wait_ms(112);//55ms:NG 56ms:OK + adc.write(0x06);//reset + wait_us(32);//15us:NG 16us:OK + adc.write(0x43);// write register 00h-03h + adc.write(0x81);// AIN0-GND GAIN:1 PGA:off + adc.write(0xd0);// 2000sps turbomode single temp:off burn:off + adc.write(0xc0);// ref:AVDD + adc.write(0x00);// DOUT/DRDY + adc.write(0x08);// START + cs_adc = _Disable;// ADC chip select off + wait(0.7); + pc.printf(".\r\n"); +} + +void ind(){ + switch(INDICATOR){ + case 0: + pc.printf("A-FRONT\r\n"); + KILL_COUNT = 5752;//6252 + break; + + case 1: + pc.printf("B-REAR\r\n"); + KILL_COUNT = 5752; + break; + } + wait(1); +} + +void rstRx(){ + char ch; + ch = pc.getc();//1文字受信バッファより取り出し + if(ch == 114){ + NVIC_SystemReset(); + } +} + +void adcwork(){ + cs_adc = _Enable; + adc.write(0x10); + for(int j=0;j<3;j++){ + rch[j] = adc.write(0xff); + }; + data[buff_index][buff_depth].acc[adcsel] = (int)((rch[0]<<16)|(rch[1]<<8)|rch[2]); + data[buff_index][buff_depth].t_acc = utime.read(); + data[buff_index][buff_depth].m_t_pulse = t_pulse; + buff_depth++; + + adc.write(0x40);// write register 00h + switch(adcsel){ + case 0: + adc.write(0x91);// AIN1-GND GAIN:1 PGA:off + adcsel=1; + break; + + case 1: + adc.write(0xa1);// AIN2-GND GAIN:1 PGA:off + adcsel=2; + break; + + case 2: + adc.write(0xb1);// AIN3-GND GAIN:1 PGA:off + adcsel=3; + break; + + case 3: + adc.write(0x81);// AIN0-GND GAIN:1 PGA:off + adcsel=0; + break; + } + + if(buff_depth == BUFF_DEPTH){ + write_ok[buff_index]=true; + buff_index++; + buff_depth=0; + if(buff_index==BUFF_COUNT){ + buff_index=0; + } + } + + adc.write(0x08); //start + cs_adc = _Disable; // ADC chip select off + +} + +void gps_get(){ + double gps_time; + float lat,lng; + int stat,lock,gpsc; +// int flg =0; + char ns,ew; + gpsc =0; + while(1){ + gpsc++; + int gps_num=0; + char gps_data[256]; + memset(gps_data, '\0' ,256);; + + while(gps.getc()!='$'){} + while( (gps_data[gps_num] =gps.getc() ) !='\r'){ + gps_num++; + if(gps_num==155){//max 255 + pc.printf("\r\n**ERROR!***\r\n"); + break; + } + } +// if(gps_num==255){ +// break; +// } + + gps_data[gps_num]='\0'; + + if(sscanf(gps_data,"GPGGA,%lf,%f,%c,%f,%c,%d,%d",&gps_time,&lat,&ns,&lng,&ew,&stat,&lock) >= sum ){ + sum = 1; + pc.printf("\r\ngpstime,%lf,lat,%f,lng,%f\r\n",gps_time,lat,lng); + break; + }else{ + if( gpsc>10){ + pc.printf("NOT FOUND\r\n"); + flg = 1; + break; + } + } + } +} + +void pps_rise(){ + __disable_irq(); + t_pulse = utime.read(); + __enable_irq(); +} + +int main(void){ + pc.baud(460800); + pc.attach(rstRx, Serial::RxIrq); + wait(0.1); + pc.printf("\r\nINITIALIZING"); + cs_adc = _Disable; + adc.format(8,1); + adc.frequency(750000); + pc.printf("."); + wait(0.7); + memset(data, '\0' ,sizeof(data)); + memset(write_ok, '\0' ,sizeof(write_ok)); + adc_init(); + + ind(); + wait(0.7); + gps_get(); + wait(0.3); + pc.printf("STARTING"); + wait(0.3); + pc.printf("."); + wait(0.3); + pc.printf("."); + wait(0.3); + pc.printf(".\r\n"); + + utime.start(); + utime.reset(); + + PPS.rise(pps_rise); + adc_ticker.attach_us(&adcwork,833); + + int x_buff; + int y_buff; + int z_buff; + int a_buff; + + while(1){ + if(write_ok[cur_buff]){ + for(int i=0;i<BUFF_DEPTH;i++){ + for(int j = 0 ; j < 4 ; j++){ + if(data[cur_buff][i].acc[j] != 0){ + x_buff = data[cur_buff][i].acc[j]; + pc.printf("x,%d,%f,%f\r\n",x_buff,data[cur_buff][i].t_acc,data[cur_buff][i].m_t_pulse); + } + + else if(data[cur_buff][i].acc[j] != 0){ + y_buff = data[cur_buff][i].acc[j]; + pc.printf("y,%d,%f,%f\r\n",y_buff,data[cur_buff][i].t_acc,data[cur_buff][i].m_t_pulse); + } + + else if(data[cur_buff][i].acc[j] != 0){ + z_buff = data[cur_buff][i].acc[j]; + pc.printf("z,%d,%f,%f\r\n",z_buff,data[cur_buff][i].t_acc,data[cur_buff][i].m_t_pulse); + } + + else{ + a_buff = data[cur_buff][i].acc[j]; + pc.printf("a,%d,%f,%f\r\n",a_buff,data[cur_buff][i].t_acc,data[cur_buff][i].m_t_pulse); + } + } + } + + write_ok[cur_buff]=false; + cur_buff++; + kill++; + if(cur_buff==BUFF_COUNT){ + cur_buff=0; + } + if(kill == KILL_COUNT){ + adc_ticker.detach(); + } + if(kill == KILL_COUNT+1){ + wait(40); + gps_get(); + float stamp=utime.read(); + pc.printf("%f\r\n",stamp); + } + } + } +}