NAN CAO
Output the angle data of Encoder
Dependencies: AD5754 FatFileSystem Impact_nagano_0630 MSCFileSystem QEI mbed
Diff: main.cpp
- Revision:
- 2:8b24c7cfc9b2
- Parent:
- 1:f37787740470
--- a/main.cpp Sat Jul 16 00:19:40 2016 +0000
+++ b/main.cpp Tue Oct 11 03:38:51 2016 +0000
@@ -1,172 +1,65 @@
#include "mbed.h"
#include "QEI.h" // for encoder
-#include "USBHostMSD.h" // for USB memory
+#include "MSCFileSystem.h"
#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 1024 // 5(kHz) * 0.2(s) = 1000
-
#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(){
+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);
+int main()
+{
+ pc.baud(115200);
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'){
+ 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);
-
+ pc.printf("\nHAPTIC test start\n");
wait(0.5);
pc.printf("Start initialization\nLift device\n\n");
wait(0.5);
@@ -177,189 +70,32 @@
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 < -350){
- 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)(32768)); // 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=VEL_WINDOW-2; i>=0; 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(print_cnt > 1000000/ENCODER_PERIOD_US){
- printf("velocity = %d\n", encoder_vel);
- print_cnt = 0;
- }
- print_cnt++;
-
- //if(encoder_cnt < 1 && encoder_vel >10){
- if(encoder_cnt > -1 && encoder_vel == -16){ //
- state = AFTER_IMPACT_STATE;
- pc.printf("Collision!!\n\n");
- printf("Collision velocity = %d\n", encoder_vel);
- start_time = t.read();
- save_cnt = 100;
-
- DAdata[0] = (uint16_t)((int16_t)(50000)); // 16bit; 0 ~ 32768 ~ 65535 (-5 ~0 ~ 5(V))
- if(DAdata[0] > 65536){
- DAdata[0] = 0;
- }
- channel = 0;
- dac.send(channel, DAdata[0]);
-
- }
-
- /*
- if(print_cnt > 1000000/ENCODER_PERIOD_US){
- printf("encoder = %d\n", encoder_cnt);
- print_cnt = 0;
- }
- print_cnt++;
- */
- }
- break;
- case AFTER_IMPACT_STATE:
- DAdata[0] = (uint16_t)((int16_t)(50000)); // 16bit; 0 ~ 32768 ~ 65535 (-5 ~0 ~ 5(V))
- if(DAdata[0] > 65536){
- DAdata[0] = 0;
- }
- channel = 0;
- dac.send(channel, DAdata[0]);
-
- /*
- if(vibrator_flag){
- vibrator_flag = false;
- //vibrator = vib_cmd;
- }
- */
-
- pc.printf("'R' key: Restart\n\n");
- while(state == AFTER_IMPACT_STATE) {
- if(pc.readable()){
- if(pc.getc() == 'r'){
- state = INITIALISATION_STATE;
- }
- }
- }
- break;
- default:
- state = INITIALISATION_STATE;
- break;
- }
+ while(main_flag)
+ {
+ if(encoder_flag)
+ {
+ encoder_flag = false;
+ encoder_cnt = encoder.getPulses(); // One of the encoders needs a sign change
- /*
- 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;
+ 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(print_cnt > 1000000/ENCODER_PERIOD_US)
+ {
+ printf("encoder = %d\n", encoder_ave);
+ printf("time = %f\n", t.read());
+ print_cnt = 0;
+ }
+ print_cnt++;
+ }
}
- pc.printf("HAPTIC test stop\n\n\n");
- wait(1);
-
- while(end_flag){
-
- FILE *fp;
- fp = fopen("/usb/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");
+
}