naoki kaneko
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);
+ }
+ }
+ }
+}