Revision 21:8802034b7810, committed 2017-01-06

Comitter:

tsumagari

Date:

Fri Jan 06 11:41:32 2017 +0000

Branch:

Thread????

Parent:

19:fa3f9ba17af8

Child:

22:5cbebf097600

Commit message:

Serial?RawSerial??????

Changed in this revision

--- a/Cadence.h	Sat Dec 17 09:12:39 2016 +0000
+++ b/Cadence.h	Fri Jan 06 11:41:32 2017 +0000
@@ -4,7 +4,7 @@
 #include "mbed.h"
 #include <string>
 
-class Cadence : public Serial{
+class Cadence : public RawSerial{
     private:
     static const int DATAS_NUM = 75;
     
@@ -16,7 +16,7 @@
     string strData;
     int data_count, data_num;
     double cadence, voltage;
-    Cadence(PinName tx, PinName rx, const char* name = NULL) : Serial(tx, rx, NULL){
+    Cadence(PinName tx, PinName rx, const char* name = NULL) : RawSerial(tx, rx){
         for(int i=0;i<DATAS_NUM;i++) data[i]= NULL;
         data_num=0;
         data_count=0;

--- a/main.cpp	Sat Dec 17 09:12:39 2016 +0000
+++ b/main.cpp	Fri Jan 06 11:41:32 2017 +0000
@@ -19,11 +19,11 @@
 #define SD_WRITE_NUM 10
 #define INIT_SERVO_PERIOD_MS 20
 
-//Cadence cadence(p13,p14);
+Cadence cadence(p13,p14);
 //Ticker cadenceTicker;
 
-Serial pc(USBTX,USBRX);
-Serial Android(p13,p14);
+RawSerial pc(USBTX,USBRX);
+RawSerial Android(p13,p14);
 BufferedSoftSerial twe(p9,p10);
 BufferedSoftSerial soudaSerial(p17,p18);
 //Ticker writeDatasTicker;
@@ -64,6 +64,7 @@
 void init();
 void FusokukeiInit();
 void SdInit();
+void SDprintf(void const *argument);
 void DataReceiveFromSouda();
 void WriteDatas();
 float calcAttackAngle();
@@ -122,18 +123,21 @@
     }
 }
 
-void SDprintf(){
-    fp = fopen("/sd/mydir/sdtest.csv", "a");
-    if(fp == NULL) {
-        error("Could not open file for write\n");
+void SDprintf(void const *argument){
+    if(write_datas_index == SD_WRITE_NUM-1){
+        fp = fopen("/sd/mydir/sdtest.csv", "a");
+        if(fp == NULL) {
+            error("Could not open file for write\n");
+        }
+        for(int i = 0; i < SD_WRITE_NUM; i++){
+            for(int j = 0; j < WRITE_DATAS_NUM; j++){
+                fprintf(fp,"%f,", writeDatas[i][j]);
+            }
+        }
+        fprintf(fp,"\n\r");
+        fclose(fp);
+        write_datas_index=0;
     }
-    for(int i = 0; i < SD_WRITE_NUM; i++){
-        for(int j = 0; j < WRITE_DATAS_NUM; j++){
-            fprintf(fp,"%f,", writeDatas[i][j]);
-        }
-    }
-    fprintf(fp,"\n\r");
-    fclose(fp);
 }
 
 void WriteDatas(){
@@ -158,7 +162,7 @@
 //    pc.printf("\n\r");
 //    twe.printf("\n\r");
     if(write_datas_index == SD_WRITE_NUM-1){
-        SDprintf();
+//        SDprintf();
         write_datas_index=0;
     }
     else{

--- a/main.cpp.orig	Sat Dec 17 09:12:39 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,315 +0,0 @@
-//計器プログラム
-#include "mbed.h"
-#include "Fusokukei.h"
-#include "MPU6050.h"
-#include "SDFileSystem.h"
-#include "BufferedSoftSerial.h"
-#include "Cadence.h"
-
-#define KX_VALUE_MIN 0.4
-#define KX_VALUE_MAX 0.8
-#define SOUDA_DATAS_NUM 12
-#define WRITE_DATAS_NUM 20
-#define MPU_LOOP_TIME 0.01
-#define AIR_LOOP_TIME 0.01
-#define WRITE_DATAS_LOOP_TIME 1
-#define ROLL_R_MAX_DEG 2
-#define ROLL_L_MAX_DEG 2
-#define MPU_DELT_MIN 250
-#define SD_WRITE_NUM 10
-#define INIT_SERVO_PERIOD_MS 20
-
-//Cadence cadence(p13,p14);
-//Ticker cadenceTicker;
-
-Serial pc(USBTX,USBRX);
-BufferedSoftSerial twe(p9,p10);
-//Serial soudaSerial(p13,p14);
-BufferedSoftSerial soudaSerial(p17,p18);
-//Ticker writeDatasTicker;
-Timer writeTimer;
-
-InterruptIn FusokukeiPin(p21);
-Ticker FusokukeiTicker;
-Fusokukei air;
-volatile int air_kaitensu= 0;
-
-float sum = 0;
-uint32_t sumCount = 0;
-MPU6050 mpu6050;
-Timer t;
-Ticker mpu6050Ticker;
-
-AnalogIn kx_X(p17);
-AnalogIn kx_Y(p16);
-AnalogIn kx_Z(p15);
-float KX_X,KX_Y,KX_Z;
-
-DigitalOut RollAlarmR(p20);
-DigitalOut RollAlarmL(p19);
-DigitalOut led(LED1);
-DigitalOut led2(LED2);
-
-SDFileSystem sd(p5, p6, p7, p8, "sd");
-FILE* fp;
-
-PwmOut kisokuServo(p26);
-PwmOut geikakuServo(p22);
-
-char soudaDatas[SOUDA_DATAS_NUM];
-float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
-volatile int write_datas_index = 0;
-
-void cadenceDataReceive();
-void air_countUp();
-void call_calcAirSpeed();
-void init();
-void FusokukeiInit();
-void MpuInit();
-void mpuProcessing();
-//void cadenceRead();
-//void cadenceInit();
-void SdInit();
-void DataReceiveFromSouda();
-void WriteDatas();
-float calcAttackAngle();
-float calcKXdeg(float x);
-
-void cadenceDataReceive(){
-//    cadence.readData();
-}
-
-void air_countUp(){
-    air_kaitensu++;
-}
-
-void call_calcAirSpeed(){
-    air.calcAirSpeed(air_kaitensu);
-    air_kaitensu = 0;
-}
-
-void init(){
-    twe.baud(19200);
-    soudaSerial.baud(9600);
-//    cadenceTicker.attach(&cadenceDataReceive,0.1);
-    //writeTimer.start();
-    kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
-    geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
-    FusokukeiInit();
-    MpuInit(); 
-    SdInit();
-    //writeDatasTicker.attach(&WriteDatas,1);
-//    soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq);
-}
-
-void FusokukeiInit(){
-    FusokukeiPin.rise(air_countUp);
-    FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
-}
-
-void MpuInit(){
-    i2c.frequency(400000);  // use fast (400 kHz) I2C
-    t.start();
-    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
-    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
-        wait(1);
-        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
-        wait(1);
-        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
-            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
-            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
-            mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
-            wait(2);
-        } else {
-        }
-    } else {
-        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
-    }   
-}
-
-double calcPulse(int deg){
-    return (0.0006+(deg/180.0)*(0.00235-0.00045));
-
-}
-
-void mpuProcessing(){
-    if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
-        mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
-        mpu6050.getAres();
-        ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
-        ay = (float)accelCount[1]*aRes - accelBias[1];
-        az = (float)accelCount[2]*aRes - accelBias[2];
-        mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
-        mpu6050.getGres();
-        gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
-        gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
-        gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
-        tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
-        temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
-    }
-    Now = t.read_us();
-    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
-    lastUpdate = Now;
-    sum += deltat;
-    sumCount++;
-    if(lastUpdate - firstUpdate > 10000000.0f) {
-        beta = 0.04;  // decrease filter gain after stabilized
-        zeta = 0.015; // increasey bias drift gain after stabilized
-    }
-    mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
-    delt_t = t.read_ms() - count;
-    if (delt_t > MPU_DELT_MIN) {
-        yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-        pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-        roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-        pitch *= 180.0f / PI;
-        yaw   *= 180.0f / PI;
-        roll  *= 180.0f / PI;
-        myled= !myled;
-        count = t.read_ms();
-        sum = 0;
-        sumCount = 0;
-    }
-}
-
-
-void SdInit(){
-    mkdir("/sd/mydir", 0777);
-    fp = fopen("/sd/mydir/sdtest2.csv", "w");
-    if(fp == NULL) {
-        error("Could not open file for write\n");
-    }
-    fprintf(fp, "Hello fun SD Card World!\n\r");
-    fclose(fp);
-}
-
-void DataReceiveFromSouda(){
-    led2 = !led2;
-    //pc.printf("received\n\r");
-//    bool kaigyo=0;
-    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-        if(soudaSerial.readable()) {
-            soudaDatas[i] = (char)soudaSerial.getc();
-            if(soudaDatas[i]==';') i=-1;
-//            else pc.printf("%5d:%3d",i,soudaDatas[i]);
-//            kaigyo =1;
-        }else i--;
-    }
-//    if(kaigyo) pc.printf("\n\r");
-}
-
-void SDprintf(){
-    fp = fopen("/sd/mydir/sdtest.csv", "a");
-    if(fp == NULL) {
-        error("Could not open file for write\n");
-    }
-    for(int i = 0; i < SD_WRITE_NUM; i++){
-        for(int j = 0; j < WRITE_DATAS_NUM; j++){
-            fprintf(fp,"%f,", writeDatas[i][j]);
-        }
-    }
-    fprintf(fp,"\n\r");
-    fclose(fp);
-}
-
-void WriteDatas(){
-    int i;
-    for(i = 0; i < SOUDA_DATAS_NUM; i++){
-        //writeDatas[write_datas_index][i] = 0.0;    
-        writeDatas[write_datas_index][i] = (float)soudaDatas[i];
-    }
-//    writeDatas[write_datas_index][i++] = cadence.cadence;
-    writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
-    writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
-    writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
-    writeDatas[write_datas_index][i++] = pitch;
-    writeDatas[write_datas_index][i++] = roll;
-    writeDatas[write_datas_index][i++] = yaw;
-    writeDatas[write_datas_index][i++] = airSpeed;
-    //writeDatas[write_datas_index][i++] = writeTimer.read();
-    //for(i = 0; i < WRITE_DATAS_NUM; i++){
-//        pc.printf("%f   ", writeDatas[write_datas_index][i]);
-//        twe.printf("%f,", writeDatas[write_datas_index][i]);
-//    }
-//    pc.printf("\n\r");
-//    twe.printf("\n\r");
-    if(write_datas_index == SD_WRITE_NUM-1){
-        SDprintf();
-        write_datas_index=0;
-    }
-    else{
-        write_datas_index++;
-    } 
-    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-        pc.printf("%i   ",soudaDatas[i]);
-        twe.printf("%i,",soudaDatas[i]);
-    }
-//    twe.printf("%f\n\r",cadence.cadence);
-//    pc.printf("%f\n\r",cadence.cadence);
-    //pc.printf("\n\r");
-    twe.printf("%f,%f,%f,",pitch,roll,yaw);
-    twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
-    twe.printf("%f,\r\n",airSpeed); 
-    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
-    //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
-    pc.printf("%f\n\r",airSpeed);
-    //SDprintf();
-}
-
-void WriteDatasF(){
-    pc.printf("airSpeed:%f\n\r",airSpeed);
-}
-
-float calcKXdeg(float x){
-    return -310.54*x+156.65;
-}
-
-float calcAttackAngle(){
-    return pitch-calcKXdeg(kx_Z.read());
-}
-
-void RollAlarm(){ 
-    if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
-        RollAlarmL = 1;
-    }
-    else{
-        RollAlarmL = 0;
-    }
-    
-    if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
-        RollAlarmR = 1;
-    }
-    else{
-        RollAlarmR = 0;
-    }
-}
-
-void WriteServo(){
-    //kisokuServo.pulsewidth(calcPulse(airSpeed*10));
-    kisokuServo.pulsewidth(calcPulse(9*airSpeed));
-    if(pitch<0){
-        geikakuServo.pulsewidth(calcPulse(0));
-    }
-    else{
-        geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
-    }
-    //pc.printf("a:%f",airSpeed);
-    //pc.printf("p:%f\r\n",pitch);
-    //kisokuServo.pulsewidth(calcPulse(0));
-    //geikakuServo.pulsewidth(calcPulse(0));
-}
-
-int main(){
-    init();
-    while(1){
-        pc.printf("test\n\r");
-//        if(test.readable()) pc.printf("%d   ",test.getc());
-//        pc.printf("<-softserial\n\r");
-        mpuProcessing();
-        RollAlarm();
-        DataReceiveFromSouda();
-//        cadence.readData();
-        WriteDatas();
-        WriteServo();
-    }
-}
\ No newline at end of file