NAN CAO
Output the angle data of Encoder
Dependencies: AD5754 FatFileSystem Impact_nagano_0630 MSCFileSystem QEI mbed
Revision 0:27283d71db7d, committed 2016-07-02
- Comitter:
- hikaru24
- Date:
- Sat Jul 02 11:03:56 2016 +0000
- Child:
- 1:f37787740470
- Commit message:
- for cao
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AD5754.lib Sat Jul 02 11:03:56 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/touch_tohoku/code/AD5754/#276920e8795f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Sat Jul 02 11:03:56 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBHost.lib Sat Jul 02 11:03:56 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/touch_tohoku/code/USBHost/#7e72b57ebb8c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jul 02 11:03:56 2016 +0000
@@ -0,0 +1,353 @@
+#include "mbed.h"
+#include "QEI.h" // for encoder
+#include "USBHostMSD.h" // for USB memory
+#include "AD5754.h" // for DA converter
+
+#define PIEZO_PERIOD_US 1000 // 1kHz
+#define ENCODER_PERIOD_US 1000 // 1kHz
+//#define ENCODER_PERIOD_US 100000 // 10Hz
+//#define VIBRATOR_PERIOD_US 200 // 5kHz
+//#define ACC_PERIOD_US 200 //5kHz
+
+//// State in main phase ////
+#define INITIALISATION_STATE 0
+#define BEFORE_IMPACT_STATE 1
+#define AFTER_IMPACT_STATE 2
+
+#define RECORD_PERIOD 1.0
+
+#define SAVE_NUMBER 2048
+
+#define AVE_WINDOW 10
+#define VEL_WINDOW 10
+
+Serial pc(USBTX, USBRX);
+
+AnalogIn piezo(p19);
+QEI encoder(p21, p22, NC, 2000); // encoder
+AnalogOut vibrator(p18);
+AD5754 dac(p5, NC, p7, p8); // spi_(mosi, miso, sck), nCS_(cs)
+
+
+Ticker piezo_t;
+Ticker encoder_t;
+//Ticker vibrator_t;
+//Ticker acc_t;
+
+Timer t;
+
+volatile bool piezo_flag = false;
+void piezo_interrupt(){
+ piezo_flag = true;
+}
+
+volatile bool encoder_flag = false;
+void encoder_interrupt(){
+ encoder_flag = true;
+}
+
+/*
+volatile bool vibrator_flag = false;
+void vibrator_interrupt(){
+ vibrator_flag = true;
+}
+*/
+/*
+volatile bool acc_flag = false;
+void acc_interrupt(){
+ acc_flag = true;
+}
+*/
+
+int main() {
+
+ pc.baud(115200);
+
+ USBHostMSD msd("usb");
+
+ while(!msd.connect()){
+ wait(1);
+ }
+
+ piezo_t.attach_us(&piezo_interrupt, PIEZO_PERIOD_US);
+ encoder_t.attach_us(&encoder_interrupt, ENCODER_PERIOD_US);
+ //vibrator_t.attach_us(&vibrator_interrupt, VIBRATOR_PERIOD_US);
+ //acc_t.attach_us(&acc__interrupt, ACC_PERIOD_US);
+
+ uint16_t DAdata[3] = {}; // 16bit; 0 ~ 32768 ~ 65535 (-5 ~0 ~ 5(V))
+ int channel = 0;
+
+
+ bool start_flag = true;
+ bool main_flag = false;
+ bool end_flag = false;
+
+ short state = INITIALISATION_STATE;
+
+ //// Time ////
+ float start_time;
+
+ //// Piezosensor ////
+ float piezo_val = 0.0;
+ //float piezo_val_buf[10] = {};
+
+ //// Encoder ////
+ int encoder_cnt = 0;
+ int encoder_buf[AVE_WINDOW] = {};
+ int encoder_ave = 0;
+ int encoder_ave_buf[VEL_WINDOW] = {};
+ int encoder_vel = 0;
+
+ int i = 0;
+ int j = 0;
+ int buf[500] = {};
+ int data = 0;
+ int ret;
+
+ /*
+ //// Vibrator ////
+ float vib_cmd = 0.5;
+ */
+
+ //// Accelerometer ////
+ /*
+ int acc_val[3] = {};
+ float acc_fval[3] {};
+ float acc_fval_xbuf[5] = {};
+ float acc_fval_ybuf[5] = {};
+ float acc_fval_zbuf[5] = {};
+ */
+
+ //// Data buff ////
+ float save_data[SAVE_NUMBER] = {};
+ int save_cnt;
+
+ LocalFileSystem local("local");
+
+ //// Option ////
+ int print_cnt = 0;
+
+
+ // Fix vibrator
+ vibrator = 0.0;
+
+ pc.printf("'S' key: Start state\n\n");
+ //pc.printf("'E' key: End state\n\n");
+ wait(1);
+
+ dac.rangeSelect();
+ dac.setPowerControl();
+
+
+ while(start_flag){
+ if(pc.readable()){
+ if(pc.getc() == 's'){
+ start_flag = false;
+ pc.printf("\nHAPTIC test start\n");
+
+ pc.printf("\nFile reading:\n");
+ FILE *fp = fopen( "/usb/test.csv", "r");
+
+ if ( fp == NULL )
+ {
+ error("\nCould not open file\n");
+ }
+
+ /*
+ while( (ret=fscanf(fp, "%d", &data)) != EOF){
+ buf[i] = data;
+ i++;
+ pc.printf("\nError\n");
+ }
+ */
+
+
+ fclose(fp);
+
+ wait(0.5);
+ pc.printf("Start initialization\nLift device\n\n");
+ wait(0.5);
+ main_flag = true;
+ }
+ }
+ }
+
+ t.start();
+
+ while(main_flag){
+ switch(state){
+ case INITIALISATION_STATE:
+ if(encoder_flag){
+ vibrator = 0.5;
+ encoder_flag = false;
+ encoder_cnt = encoder.getPulses(); // One of the encoders needs a sign change
+
+ encoder_ave = encoder_cnt;
+ for(int i=0; i<AVE_WINDOW-1; i++){
+ encoder_buf[i+1] = encoder_buf[i];
+ encoder_ave += encoder_buf[i];
+ }
+ encoder_buf[0] = encoder_cnt;
+ encoder_ave = encoder_ave/AVE_WINDOW;
+
+ if(encoder_ave > 600){
+ //state = BEFORE_IMPACT_STATE;
+ //pc.printf("Finish initialization\n\n");
+ }
+
+
+ if(print_cnt > 1000000/ENCODER_PERIOD_US){
+ printf("encoder = %d\n", encoder_ave);
+ print_cnt = 0;
+ }
+ print_cnt++;
+
+ ////// DA converter //////
+ DAdata[0] = (uint16_t)((int16_t)(65535)); // 16bit; 0 ~ 32768 ~ 65535 (-5 ~0 ~ 5(V))
+ if(DAdata[0] > 65536){
+ DAdata[0] = 0;
+ }
+ channel = 0;
+ dac.send(channel, DAdata[0]);
+ //////
+
+ }
+ break;
+ case BEFORE_IMPACT_STATE:
+
+ if(piezo_flag){
+ piezo_flag = false;
+ piezo_val = piezo; // 0.0(0.0V)~ 0.5(1.65V) ~ 1.0(3.3V)
+ /*
+ if(print_cnt > 1000000/PIEZO_PERIOD_US){
+ //printf("piezosensor = %0.3f\n", piezo_val);
+ //printf("time = %f\n", t.read());
+ print_cnt = 0;
+ }
+ print_cnt++;
+ */
+
+ // Save piezo data in save_data[0] ~ [99]
+ save_data[100] = piezo_val;
+ for(save_cnt=0; save_cnt<100; save_cnt++){
+ save_data[save_cnt] = save_data[save_cnt+1];
+ }
+
+ }
+ if(encoder_flag){
+ encoder_flag = false;
+ encoder_cnt = encoder.getPulses(); // One of the encoders needs a sign change
+
+ /*
+ encoder_ave = encoder_cnt;
+ for(int i=0; i<AVE_WINDOW-1; i++){
+ encoder_buf[i+1] = encoder_buf[i];
+ encoder_ave += encoder_buf[i];
+ }
+ encoder_buf[0] = encoder_cnt;
+ encoder_ave = encoder_ave/AVE_WINDOW;
+ */
+
+ /*
+ for(int i=0; i<VEL_WINDOW-1; i++){
+ encoder_ave_buf[i+1] = encoder_ave_buf[i];
+ }
+ encoder_ave_buf[0] = encoder_ave;
+ encoder_vel = encoder_ave_buf[VEL_WINDOW-1] - encoder_ave_buf[0];
+ */
+
+ //if(encoder_cnt < 1 && encoder_vel >10){
+ if(encoder_cnt < 1){
+ state = AFTER_IMPACT_STATE;
+ pc.printf("Collision!!\n\n");
+ start_time = t.read();
+ save_cnt = 100;
+ }
+
+ /*
+ if(print_cnt > 1000000/ENCODER_PERIOD_US){
+ printf("encoder = %d\n", encoder_cnt);
+ print_cnt = 0;
+ }
+ print_cnt++;
+ */
+ }
+ break;
+ case AFTER_IMPACT_STATE:
+ if(t.read() > start_time + RECORD_PERIOD){
+ main_flag = false;
+ end_flag = true;
+ }
+ if(piezo_flag){
+ piezo_flag = false;
+ piezo_val = piezo; // 0.0(0.0V)~ 0.5(1.65V) ~ 1.0(3.3V)
+ /*
+ if(print_cnt > 1000000/PIEZO_PERIOD_US){
+ //printf("piezosensor = %0.3f\n", piezo_val);
+ //printf("time = %f\n", t.read());
+ print_cnt = 0;
+ }
+ print_cnt++;
+ */
+
+ // Save piezo data in save_data[100] ~ [2047]
+ if(save_cnt < SAVE_NUMBER){
+ save_data[save_cnt] = piezo_val;
+ save_cnt++;
+ }
+
+ }
+ /*
+ if(vibrator_flag){
+ vibrator_flag = false;
+ //vibrator = vib_cmd;
+ }
+ */
+ break;
+ default:
+ state = INITIALISATION_STATE;
+ break;
+ }
+
+ /*
+ if(acc_flag){
+ acc_flag = false;
+ accelerometer.getOutput(acc_val);
+ acc_fval[0] = (float)(int16_t)acc_val[0];
+ acc_fval[1] = (float)(int16_t)acc_val[1];
+ acc_fval[2] = (float)(int16_t)acc_val[2];
+ //printf("x = %6.2f y = %6.2f z = %6.2f\n", acc_fval[0]*200/4096, acc_fval[1]*200/4096, acc_fval[2]*200/4096);
+ }
+ */
+
+ if(pc.readable()){
+ if(pc.getc() == 'e'){
+ main_flag = false;
+ end_flag = true;
+ }
+ }
+
+ }
+ pc.printf("HAPTIC test stop\n\n\n");
+ wait(1);
+
+ while(end_flag){
+ /*
+ FILE *fp;
+ fp = fopen("/local/hap_test.csv", "w");
+ if(fp == NULL){
+ error("Could not open file for write\n");
+ }
+ else{
+ for(int i=0; i<SAVE_NUMBER-1; i++){
+ fprintf(fp,"%f\n",save_data[i]);
+ }
+ fclose(fp);
+ }
+ */
+ end_flag = false;
+ }
+ printf("END Haptic test\n");
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Jul 02 11:03:56 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34 \ No newline at end of file