naoki kaneko / Mbed 2 deprecated z00_NSYS_CAR_1220

test

Dependencies:   z00_NSYS_CAR mbed SDFileSystem

main1220.cpp@1:32dfdcc9490f, 2020-12-23 (annotated)

Committer:
NAOKI19990810
Date:
Wed Dec 23 08:49:47 2020 +0000
Revision:
1:32dfdcc9490f
huji

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NAOKI19990810 1:32dfdcc9490f 1 /*=========================================
NAOKI19990810 1:32dfdcc9490f 2 Title:4axis
NAOKI19990810 1:32dfdcc9490f 3
NAOKI19990810 1:32dfdcc9490f 4 Revision:1.0.0
NAOKI19990810 1:32dfdcc9490f 5
NAOKI19990810 1:32dfdcc9490f 6 Editor:Dyson
NAOKI19990810 1:32dfdcc9490f 7
NAOKI19990810 1:32dfdcc9490f 8 For GR-PEACH
NAOKI19990810 1:32dfdcc9490f 9 Compile using Mbed Online Compiler (revision:130)
NAOKI19990810 1:32dfdcc9490f 10
NAOKI19990810 1:32dfdcc9490f 11 reminder:
NAOKI19990810 1:32dfdcc9490f 12 Last Modified:2018/09/20
NAOKI19990810 1:32dfdcc9490f 13 ==========================================*/
NAOKI19990810 1:32dfdcc9490f 14
NAOKI19990810 1:32dfdcc9490f 15 //=====CONSTANT VARIBLES========
NAOKI19990810 1:32dfdcc9490f 16 #define INDICATOR 1//Front:0 Rear:1
NAOKI19990810 1:32dfdcc9490f 17 #define BUFF_DEPTH 128
NAOKI19990810 1:32dfdcc9490f 18 #define BUFF_COUNT 3
NAOKI19990810 1:32dfdcc9490f 19
NAOKI19990810 1:32dfdcc9490f 20 //======GLOBAL VARIBLES=========
NAOKI19990810 1:32dfdcc9490f 21 char adcsel = 0;
NAOKI19990810 1:32dfdcc9490f 22
NAOKI19990810 1:32dfdcc9490f 23 int sum = 7;
NAOKI19990810 1:32dfdcc9490f 24
NAOKI19990810 1:32dfdcc9490f 25 int kill = 0;
NAOKI19990810 1:32dfdcc9490f 26 int KILL_COUNT;
NAOKI19990810 1:32dfdcc9490f 27 float t_pulse = 0;
NAOKI19990810 1:32dfdcc9490f 28
NAOKI19990810 1:32dfdcc9490f 29 int buff_depth = 0;
NAOKI19990810 1:32dfdcc9490f 30 int buff_index = 0;
NAOKI19990810 1:32dfdcc9490f 31 int cur_buff=0;
NAOKI19990810 1:32dfdcc9490f 32
NAOKI19990810 1:32dfdcc9490f 33 int flg =0;
NAOKI19990810 1:32dfdcc9490f 34 typedef struct Logdata_t{
NAOKI19990810 1:32dfdcc9490f 35 int acc[4];
NAOKI19990810 1:32dfdcc9490f 36 float t_acc;
NAOKI19990810 1:32dfdcc9490f 37 float m_t_pulse;
NAOKI19990810 1:32dfdcc9490f 38 }Logdata;
NAOKI19990810 1:32dfdcc9490f 39 Logdata data[BUFF_COUNT][BUFF_DEPTH];
NAOKI19990810 1:32dfdcc9490f 40
NAOKI19990810 1:32dfdcc9490f 41 bool write_ok[BUFF_COUNT];
NAOKI19990810 1:32dfdcc9490f 42
NAOKI19990810 1:32dfdcc9490f 43 int rch[3];
NAOKI19990810 1:32dfdcc9490f 44
NAOKI19990810 1:32dfdcc9490f 45 //=========SYSTEM==============
NAOKI19990810 1:32dfdcc9490f 46 #include "mbed.h"
NAOKI19990810 1:32dfdcc9490f 47 #define _Enable 0
NAOKI19990810 1:32dfdcc9490f 48 #define _Disable 1
NAOKI19990810 1:32dfdcc9490f 49 Serial gps(P8_13,P8_11);
NAOKI19990810 1:32dfdcc9490f 50 Serial pc(USBTX,USBRX);
NAOKI19990810 1:32dfdcc9490f 51 InterruptIn PPS(P2_15);
NAOKI19990810 1:32dfdcc9490f 52 Timer utime;
NAOKI19990810 1:32dfdcc9490f 53 Ticker adc_ticker;
NAOKI19990810 1:32dfdcc9490f 54 //Ticker gps_ticker;
NAOKI19990810 1:32dfdcc9490f 55 SPI adc(P10_14, P10_15, P10_12);
NAOKI19990810 1:32dfdcc9490f 56 DigitalOut cs_adc(P8_15);
NAOKI19990810 1:32dfdcc9490f 57 DigitalOut myled_r(LED1);
NAOKI19990810 1:32dfdcc9490f 58 DigitalOut myled_g(LED2);
NAOKI19990810 1:32dfdcc9490f 59 DigitalOut myled_b(LED3);
NAOKI19990810 1:32dfdcc9490f 60
NAOKI19990810 1:32dfdcc9490f 61
NAOKI19990810 1:32dfdcc9490f 62 void adc_init(void){
NAOKI19990810 1:32dfdcc9490f 63 pc.printf(".");
NAOKI19990810 1:32dfdcc9490f 64 cs_adc = _Enable;// ADC chip select on
NAOKI19990810 1:32dfdcc9490f 65 wait_ms(112);//55ms:NG 56ms:OK
NAOKI19990810 1:32dfdcc9490f 66 adc.write(0x06);//reset
NAOKI19990810 1:32dfdcc9490f 67 wait_us(32);//15us:NG 16us:OK
NAOKI19990810 1:32dfdcc9490f 68 adc.write(0x43);// write register 00h-03h
NAOKI19990810 1:32dfdcc9490f 69 adc.write(0x81);// AIN0-GND GAIN:1 PGA:off
NAOKI19990810 1:32dfdcc9490f 70 adc.write(0xd0);// 2000sps turbomode single temp:off burn:off
NAOKI19990810 1:32dfdcc9490f 71 adc.write(0xc0);// ref:AVDD
NAOKI19990810 1:32dfdcc9490f 72 adc.write(0x00);// DOUT/DRDY
NAOKI19990810 1:32dfdcc9490f 73 adc.write(0x08);// START
NAOKI19990810 1:32dfdcc9490f 74 cs_adc = _Disable;// ADC chip select off
NAOKI19990810 1:32dfdcc9490f 75 wait(0.7);
NAOKI19990810 1:32dfdcc9490f 76 pc.printf(".\r\n");
NAOKI19990810 1:32dfdcc9490f 77 }
NAOKI19990810 1:32dfdcc9490f 78
NAOKI19990810 1:32dfdcc9490f 79 void ind(){
NAOKI19990810 1:32dfdcc9490f 80 switch(INDICATOR){
NAOKI19990810 1:32dfdcc9490f 81 case 0:
NAOKI19990810 1:32dfdcc9490f 82 pc.printf("A-FRONT\r\n");
NAOKI19990810 1:32dfdcc9490f 83 KILL_COUNT = 5752;//6252
NAOKI19990810 1:32dfdcc9490f 84 break;
NAOKI19990810 1:32dfdcc9490f 85
NAOKI19990810 1:32dfdcc9490f 86 case 1:
NAOKI19990810 1:32dfdcc9490f 87 pc.printf("B-REAR\r\n");
NAOKI19990810 1:32dfdcc9490f 88 KILL_COUNT = 5752;
NAOKI19990810 1:32dfdcc9490f 89 break;
NAOKI19990810 1:32dfdcc9490f 90 }
NAOKI19990810 1:32dfdcc9490f 91 wait(1);
NAOKI19990810 1:32dfdcc9490f 92 }
NAOKI19990810 1:32dfdcc9490f 93
NAOKI19990810 1:32dfdcc9490f 94 void rstRx(){
NAOKI19990810 1:32dfdcc9490f 95 char ch;
NAOKI19990810 1:32dfdcc9490f 96 ch = pc.getc();//1文字受信バッファより取り出し
NAOKI19990810 1:32dfdcc9490f 97 if(ch == 114){
NAOKI19990810 1:32dfdcc9490f 98 NVIC_SystemReset();
NAOKI19990810 1:32dfdcc9490f 99 }
NAOKI19990810 1:32dfdcc9490f 100 }
NAOKI19990810 1:32dfdcc9490f 101
NAOKI19990810 1:32dfdcc9490f 102 void adcwork(){
NAOKI19990810 1:32dfdcc9490f 103 cs_adc = _Enable;
NAOKI19990810 1:32dfdcc9490f 104 adc.write(0x10);
NAOKI19990810 1:32dfdcc9490f 105 for(int j=0;j<3;j++){
NAOKI19990810 1:32dfdcc9490f 106 rch[j] = adc.write(0xff);
NAOKI19990810 1:32dfdcc9490f 107 };
NAOKI19990810 1:32dfdcc9490f 108 data[buff_index][buff_depth].acc[adcsel] = (int)((rch[0]<<16)|(rch[1]<<8)|rch[2]);
NAOKI19990810 1:32dfdcc9490f 109 data[buff_index][buff_depth].t_acc = utime.read();
NAOKI19990810 1:32dfdcc9490f 110 data[buff_index][buff_depth].m_t_pulse = t_pulse;
NAOKI19990810 1:32dfdcc9490f 111 buff_depth++;
NAOKI19990810 1:32dfdcc9490f 112
NAOKI19990810 1:32dfdcc9490f 113 adc.write(0x40);// write register 00h
NAOKI19990810 1:32dfdcc9490f 114 switch(adcsel){
NAOKI19990810 1:32dfdcc9490f 115 case 0:
NAOKI19990810 1:32dfdcc9490f 116 adc.write(0x91);// AIN1-GND GAIN:1 PGA:off
NAOKI19990810 1:32dfdcc9490f 117 adcsel=1;
NAOKI19990810 1:32dfdcc9490f 118 break;
NAOKI19990810 1:32dfdcc9490f 119
NAOKI19990810 1:32dfdcc9490f 120 case 1:
NAOKI19990810 1:32dfdcc9490f 121 adc.write(0xa1);// AIN2-GND GAIN:1 PGA:off
NAOKI19990810 1:32dfdcc9490f 122 adcsel=2;
NAOKI19990810 1:32dfdcc9490f 123 break;
NAOKI19990810 1:32dfdcc9490f 124
NAOKI19990810 1:32dfdcc9490f 125 case 2:
NAOKI19990810 1:32dfdcc9490f 126 adc.write(0xb1);// AIN3-GND GAIN:1 PGA:off
NAOKI19990810 1:32dfdcc9490f 127 adcsel=3;
NAOKI19990810 1:32dfdcc9490f 128 break;
NAOKI19990810 1:32dfdcc9490f 129
NAOKI19990810 1:32dfdcc9490f 130 case 3:
NAOKI19990810 1:32dfdcc9490f 131 adc.write(0x81);// AIN0-GND GAIN:1 PGA:off
NAOKI19990810 1:32dfdcc9490f 132 adcsel=0;
NAOKI19990810 1:32dfdcc9490f 133 break;
NAOKI19990810 1:32dfdcc9490f 134 }
NAOKI19990810 1:32dfdcc9490f 135
NAOKI19990810 1:32dfdcc9490f 136 if(buff_depth == BUFF_DEPTH){
NAOKI19990810 1:32dfdcc9490f 137 write_ok[buff_index]=true;
NAOKI19990810 1:32dfdcc9490f 138 buff_index++;
NAOKI19990810 1:32dfdcc9490f 139 buff_depth=0;
NAOKI19990810 1:32dfdcc9490f 140 if(buff_index==BUFF_COUNT){
NAOKI19990810 1:32dfdcc9490f 141 buff_index=0;
NAOKI19990810 1:32dfdcc9490f 142 }
NAOKI19990810 1:32dfdcc9490f 143 }
NAOKI19990810 1:32dfdcc9490f 144
NAOKI19990810 1:32dfdcc9490f 145 adc.write(0x08); //start
NAOKI19990810 1:32dfdcc9490f 146 cs_adc = _Disable; // ADC chip select off
NAOKI19990810 1:32dfdcc9490f 147
NAOKI19990810 1:32dfdcc9490f 148 }
NAOKI19990810 1:32dfdcc9490f 149
NAOKI19990810 1:32dfdcc9490f 150 void gps_get(){
NAOKI19990810 1:32dfdcc9490f 151 double gps_time;
NAOKI19990810 1:32dfdcc9490f 152 float lat,lng;
NAOKI19990810 1:32dfdcc9490f 153 int stat,lock,gpsc;
NAOKI19990810 1:32dfdcc9490f 154 // int flg =0;
NAOKI19990810 1:32dfdcc9490f 155 char ns,ew;
NAOKI19990810 1:32dfdcc9490f 156 gpsc =0;
NAOKI19990810 1:32dfdcc9490f 157 while(1){
NAOKI19990810 1:32dfdcc9490f 158 gpsc++;
NAOKI19990810 1:32dfdcc9490f 159 int gps_num=0;
NAOKI19990810 1:32dfdcc9490f 160 char gps_data[256];
NAOKI19990810 1:32dfdcc9490f 161 memset(gps_data, '\0' ,256);;
NAOKI19990810 1:32dfdcc9490f 162
NAOKI19990810 1:32dfdcc9490f 163 while(gps.getc()!='$'){}
NAOKI19990810 1:32dfdcc9490f 164 while( (gps_data[gps_num] =gps.getc() ) !='\r'){
NAOKI19990810 1:32dfdcc9490f 165 gps_num++;
NAOKI19990810 1:32dfdcc9490f 166 if(gps_num==155){//max 255
NAOKI19990810 1:32dfdcc9490f 167 pc.printf("\r\n**ERROR!***\r\n");
NAOKI19990810 1:32dfdcc9490f 168 break;
NAOKI19990810 1:32dfdcc9490f 169 }
NAOKI19990810 1:32dfdcc9490f 170 }
NAOKI19990810 1:32dfdcc9490f 171 // if(gps_num==255){
NAOKI19990810 1:32dfdcc9490f 172 // break;
NAOKI19990810 1:32dfdcc9490f 173 // }
NAOKI19990810 1:32dfdcc9490f 174
NAOKI19990810 1:32dfdcc9490f 175 gps_data[gps_num]='\0';
NAOKI19990810 1:32dfdcc9490f 176
NAOKI19990810 1:32dfdcc9490f 177 if(sscanf(gps_data,"GPGGA,%lf,%f,%c,%f,%c,%d,%d",&gps_time,&lat,&ns,&lng,&ew,&stat,&lock) >= sum ){
NAOKI19990810 1:32dfdcc9490f 178 sum = 1;
NAOKI19990810 1:32dfdcc9490f 179 pc.printf("\r\ngpstime,%lf,lat,%f,lng,%f\r\n",gps_time,lat,lng);
NAOKI19990810 1:32dfdcc9490f 180 break;
NAOKI19990810 1:32dfdcc9490f 181 }else{
NAOKI19990810 1:32dfdcc9490f 182 if( gpsc>10){
NAOKI19990810 1:32dfdcc9490f 183 pc.printf("NOT FOUND\r\n");
NAOKI19990810 1:32dfdcc9490f 184 flg = 1;
NAOKI19990810 1:32dfdcc9490f 185 break;
NAOKI19990810 1:32dfdcc9490f 186 }
NAOKI19990810 1:32dfdcc9490f 187 }
NAOKI19990810 1:32dfdcc9490f 188 }
NAOKI19990810 1:32dfdcc9490f 189 }
NAOKI19990810 1:32dfdcc9490f 190
NAOKI19990810 1:32dfdcc9490f 191 void pps_rise(){
NAOKI19990810 1:32dfdcc9490f 192 __disable_irq();
NAOKI19990810 1:32dfdcc9490f 193 t_pulse = utime.read();
NAOKI19990810 1:32dfdcc9490f 194 __enable_irq();
NAOKI19990810 1:32dfdcc9490f 195 }
NAOKI19990810 1:32dfdcc9490f 196
NAOKI19990810 1:32dfdcc9490f 197 int main(void){
NAOKI19990810 1:32dfdcc9490f 198 pc.baud(460800);
NAOKI19990810 1:32dfdcc9490f 199 pc.attach(rstRx, Serial::RxIrq);
NAOKI19990810 1:32dfdcc9490f 200 wait(0.1);
NAOKI19990810 1:32dfdcc9490f 201 pc.printf("\r\nINITIALIZING");
NAOKI19990810 1:32dfdcc9490f 202 cs_adc = _Disable;
NAOKI19990810 1:32dfdcc9490f 203 adc.format(8,1);
NAOKI19990810 1:32dfdcc9490f 204 adc.frequency(750000);
NAOKI19990810 1:32dfdcc9490f 205 pc.printf(".");
NAOKI19990810 1:32dfdcc9490f 206 wait(0.7);
NAOKI19990810 1:32dfdcc9490f 207 memset(data, '\0' ,sizeof(data));
NAOKI19990810 1:32dfdcc9490f 208 memset(write_ok, '\0' ,sizeof(write_ok));
NAOKI19990810 1:32dfdcc9490f 209 adc_init();
NAOKI19990810 1:32dfdcc9490f 210
NAOKI19990810 1:32dfdcc9490f 211 ind();
NAOKI19990810 1:32dfdcc9490f 212 wait(0.7);
NAOKI19990810 1:32dfdcc9490f 213 gps_get();
NAOKI19990810 1:32dfdcc9490f 214 wait(0.3);
NAOKI19990810 1:32dfdcc9490f 215 pc.printf("STARTING");
NAOKI19990810 1:32dfdcc9490f 216 wait(0.3);
NAOKI19990810 1:32dfdcc9490f 217 pc.printf(".");
NAOKI19990810 1:32dfdcc9490f 218 wait(0.3);
NAOKI19990810 1:32dfdcc9490f 219 pc.printf(".");
NAOKI19990810 1:32dfdcc9490f 220 wait(0.3);
NAOKI19990810 1:32dfdcc9490f 221 pc.printf(".\r\n");
NAOKI19990810 1:32dfdcc9490f 222
NAOKI19990810 1:32dfdcc9490f 223 utime.start();
NAOKI19990810 1:32dfdcc9490f 224 utime.reset();
NAOKI19990810 1:32dfdcc9490f 225
NAOKI19990810 1:32dfdcc9490f 226 PPS.rise(pps_rise);
NAOKI19990810 1:32dfdcc9490f 227 adc_ticker.attach_us(&adcwork,833);
NAOKI19990810 1:32dfdcc9490f 228
NAOKI19990810 1:32dfdcc9490f 229 int x_buff;
NAOKI19990810 1:32dfdcc9490f 230 int y_buff;
NAOKI19990810 1:32dfdcc9490f 231 int z_buff;
NAOKI19990810 1:32dfdcc9490f 232 int a_buff;
NAOKI19990810 1:32dfdcc9490f 233
NAOKI19990810 1:32dfdcc9490f 234 while(1){
NAOKI19990810 1:32dfdcc9490f 235 if(write_ok[cur_buff]){
NAOKI19990810 1:32dfdcc9490f 236 for(int i=0;i<BUFF_DEPTH;i++){
NAOKI19990810 1:32dfdcc9490f 237 for(int j = 0 ; j < 4 ; j++){
NAOKI19990810 1:32dfdcc9490f 238 if(data[cur_buff][i].acc[j] != 0){
NAOKI19990810 1:32dfdcc9490f 239 x_buff = data[cur_buff][i].acc[j];
NAOKI19990810 1:32dfdcc9490f 240 pc.printf("x,%d,%f,%f\r\n",x_buff,data[cur_buff][i].t_acc,data[cur_buff][i].m_t_pulse);
NAOKI19990810 1:32dfdcc9490f 241 }
NAOKI19990810 1:32dfdcc9490f 242
NAOKI19990810 1:32dfdcc9490f 243 else if(data[cur_buff][i].acc[j] != 0){
NAOKI19990810 1:32dfdcc9490f 244 y_buff = data[cur_buff][i].acc[j];
NAOKI19990810 1:32dfdcc9490f 245 pc.printf("y,%d,%f,%f\r\n",y_buff,data[cur_buff][i].t_acc,data[cur_buff][i].m_t_pulse);
NAOKI19990810 1:32dfdcc9490f 246 }
NAOKI19990810 1:32dfdcc9490f 247
NAOKI19990810 1:32dfdcc9490f 248 else if(data[cur_buff][i].acc[j] != 0){
NAOKI19990810 1:32dfdcc9490f 249 z_buff = data[cur_buff][i].acc[j];
NAOKI19990810 1:32dfdcc9490f 250 pc.printf("z,%d,%f,%f\r\n",z_buff,data[cur_buff][i].t_acc,data[cur_buff][i].m_t_pulse);
NAOKI19990810 1:32dfdcc9490f 251 }
NAOKI19990810 1:32dfdcc9490f 252
NAOKI19990810 1:32dfdcc9490f 253 else{
NAOKI19990810 1:32dfdcc9490f 254 a_buff = data[cur_buff][i].acc[j];
NAOKI19990810 1:32dfdcc9490f 255 pc.printf("a,%d,%f,%f\r\n",a_buff,data[cur_buff][i].t_acc,data[cur_buff][i].m_t_pulse);
NAOKI19990810 1:32dfdcc9490f 256 }
NAOKI19990810 1:32dfdcc9490f 257 }
NAOKI19990810 1:32dfdcc9490f 258 }
NAOKI19990810 1:32dfdcc9490f 259
NAOKI19990810 1:32dfdcc9490f 260 write_ok[cur_buff]=false;
NAOKI19990810 1:32dfdcc9490f 261 cur_buff++;
NAOKI19990810 1:32dfdcc9490f 262 kill++;
NAOKI19990810 1:32dfdcc9490f 263 if(cur_buff==BUFF_COUNT){
NAOKI19990810 1:32dfdcc9490f 264 cur_buff=0;
NAOKI19990810 1:32dfdcc9490f 265 }
NAOKI19990810 1:32dfdcc9490f 266 if(kill == KILL_COUNT){
NAOKI19990810 1:32dfdcc9490f 267 adc_ticker.detach();
NAOKI19990810 1:32dfdcc9490f 268 }
NAOKI19990810 1:32dfdcc9490f 269 if(kill == KILL_COUNT+1){
NAOKI19990810 1:32dfdcc9490f 270 wait(40);
NAOKI19990810 1:32dfdcc9490f 271 gps_get();
NAOKI19990810 1:32dfdcc9490f 272 float stamp=utime.read();
NAOKI19990810 1:32dfdcc9490f 273 pc.printf("%f\r\n",stamp);
NAOKI19990810 1:32dfdcc9490f 274 }
NAOKI19990810 1:32dfdcc9490f 275 }
NAOKI19990810 1:32dfdcc9490f 276 }
NAOKI19990810 1:32dfdcc9490f 277 }