main.cpp

00001 #include "mbed.h"
00002 
00003 #define PI 3.141592
00004 
00005 DigitalOut leds[8] = { PA_11,LED1, PB_2, PB_1, PB_15, PB_14, PB_13, PC_4 };
00006 I2C i2c(I2C_SDA, I2C_SCL);
00007 Serial    FEP02(PC_6, PA_12);
00008 Serial    pc(USBTX, USBRX);
00009 
00010 const int  addr[4] = { 0x10, 0x23, 0x56, 0x76 };
00011 int       i, dig, i2cVAL, error_val = 0;
00012 // double    rad, prePwmDuty[4], PwmDuty[4];
00013 
00014 int main()
00015 {
00016     char     data[5] = { 0 }, FR_motor[2], RR_motor[2], RL_motor[2], FL_motor[2];
00017     int  i, dig, i2cVAL, error = 0;
00018     double rad, prePwmDuty[4], PwmDuty[4];
00019     i2c.frequency(300000);  // I2Cの通信速度設定
00020     FEP02.baud(19200);// シリアルのボーレートを19200に設定
00021     while(1) {                  // 受信
00022         if(FEP02.getc() == 255) {
00023             for(i = 0; i < 5; i++) {
00024                 data[i]  = FEP02.getc();
00025                 error_val = 0;
00026             }
00027             if(FEP02.getc() != 127) {
00028                 error_val++;
00029             }
00030         }  else {
00031             error_val++;
00032         }
00033         if(error > 10) {    // 受信error
00034             dig = 360;
00035             pc.printf("error\r\n");
00036         }
00037         dig = data[0] + data[1] * 128;// 受信データから元の角度に変換
00038         rad = ((double) dig / 180.0) * PI;      // radに変換
00039         if(dig >= 360) {
00040             prePwmDuty[0] = 0;
00041             prePwmDuty[1] = 0;
00042             prePwmDuty[2] = 0;
00043             prePwmDuty[3] = 0;
00044             pc.printf("stop\r\n");
00045         } else {
00046             // 各モータへの出力を算出
00047             prePwmDuty[0] = sin(rad - (1.0 / 4.0) * PI);
00048             prePwmDuty[1] = sin(rad - (3.0 / 4.0) * PI);
00049             prePwmDuty[2] = sin(rad - (5.0 / 4.0) * PI);
00050             prePwmDuty[3] = sin(rad - (7.0 / 4.0) * PI);
00051         }
00052         FR_motor[0] = 127 + prePwmDuty[0] * 127;
00053         RR_motor[0] = 127 + prePwmDuty[1] * 127;
00054         RL_motor[0] = 127 + prePwmDuty[2] * 127;
00055         FL_motor[0] = 127 + prePwmDuty[3] * 127;
00056         i2cVAL    += i2c.write(addr[0], FR_motor, 2, false);
00057         i2cVAL    += i2c.write(addr[1], RR_motor, 2, false);
00058         i2cVAL    += i2c.write(addr[2], RL_motor, 2, false);
00059         i2cVAL    += i2c.write(addr[3], FL_motor, 2, false);
00060         if(i2cVAL) {
00061             leds[2] = 1;
00062         } else {
00063             leds[2] = 0;
00064         }
00065         pc.printf("%d,%d,%d,%d,%d\r\n", dig, FR_motor[0], data[2], data[3], data[4]);
00066     }
00067 }