NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated MecanumWheel6

aa

Dependencies:   mbed

main.cpp@0:4ed9b2004dd0, 2016-05-20 (annotated)

Committer:
hajime_nntc
Date:
Fri May 20 10:06:38 2016 +0000
Revision:
0:4ed9b2004dd0
hajime_mecanum_ver.1; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hajime_nntc 0:4ed9b2004dd0 1 #include "mbed.h"
hajime_nntc 0:4ed9b2004dd0 2
hajime_nntc 0:4ed9b2004dd0 3 #define PI 3.141592
hajime_nntc 0:4ed9b2004dd0 4
hajime_nntc 0:4ed9b2004dd0 5 DigitalOut leds[8] = { PA_11, PB_12, PB_2, PB_1, PB_15, PB_14, PB_13, PC_4 };
hajime_nntc 0:4ed9b2004dd0 6 I2C i2c(I2C_SDA, I2C_SCL);
hajime_nntc 0:4ed9b2004dd0 7 Serial FEP02(PC_6, PA_12);
hajime_nntc 0:4ed9b2004dd0 8 Serial pc(USBTX, USBRX);
hajime_nntc 0:4ed9b2004dd0 9
hajime_nntc 0:4ed9b2004dd0 10 const int addr[4] = { 0x10, 0x23, 0x56, 0x76 };
hajime_nntc 0:4ed9b2004dd0 11 int i,dig,i2cVAL,error_val=0;
hajime_nntc 0:4ed9b2004dd0 12 //double rad, prePwmDuty[4], PwmDuty[4];
hajime_nntc 0:4ed9b2004dd0 13
hajime_nntc 0:4ed9b2004dd0 14 int main()
hajime_nntc 0:4ed9b2004dd0 15 {
hajime_nntc 0:4ed9b2004dd0 16 char data[5] = { 0 },MDFL[2],MDFR[2],MDRL[2],MDRR[2];
hajime_nntc 0:4ed9b2004dd0 17 int i,dig,i2cVAL,error=0;
hajime_nntc 0:4ed9b2004dd0 18 double rad, prePwmDuty[4], PwmDuty[4];
hajime_nntc 0:4ed9b2004dd0 19 i2c.frequency(300000); // I2Cの通信速度設定
hajime_nntc 0:4ed9b2004dd0 20 FEP02.baud(19200);// シリアルのボーレートを19200に設定
hajime_nntc 0:4ed9b2004dd0 21 while(1) {
hajime_nntc 0:4ed9b2004dd0 22 if(FEP02.getc() == 255) {
hajime_nntc 0:4ed9b2004dd0 23 for(i = 0; i < 5; i++) {
hajime_nntc 0:4ed9b2004dd0 24 data[i] = FEP02.getc();
hajime_nntc 0:4ed9b2004dd0 25 error_val = 0;
hajime_nntc 0:4ed9b2004dd0 26 }
hajime_nntc 0:4ed9b2004dd0 27 } else {
hajime_nntc 0:4ed9b2004dd0 28 error_val++;
hajime_nntc 0:4ed9b2004dd0 29 }
hajime_nntc 0:4ed9b2004dd0 30 dig = data[0] + data[1] * 128;// 受信データから元の角度に変換
hajime_nntc 0:4ed9b2004dd0 31 rad = (dig / 180.0) * PI; // radianに変換
hajime_nntc 0:4ed9b2004dd0 32 if(error > 10) {
hajime_nntc 0:4ed9b2004dd0 33 dig = 360;
hajime_nntc 0:4ed9b2004dd0 34 pc.printf("error\r\n");
hajime_nntc 0:4ed9b2004dd0 35 }
hajime_nntc 0:4ed9b2004dd0 36 if(dig >= 360) {
hajime_nntc 0:4ed9b2004dd0 37 prePwmDuty[0] = 0;
hajime_nntc 0:4ed9b2004dd0 38 prePwmDuty[1] = 0;
hajime_nntc 0:4ed9b2004dd0 39 prePwmDuty[2] = 0;
hajime_nntc 0:4ed9b2004dd0 40 prePwmDuty[3] = 0;
hajime_nntc 0:4ed9b2004dd0 41 } else {
hajime_nntc 0:4ed9b2004dd0 42 // 各モータへの出力を算出
hajime_nntc 0:4ed9b2004dd0 43 prePwmDuty[0] = 1.41421356 * sin(rad - ((5 / 4) * PI));
hajime_nntc 0:4ed9b2004dd0 44 prePwmDuty[1] = 1.41421356 * sin(rad - (PI / 4));
hajime_nntc 0:4ed9b2004dd0 45 prePwmDuty[2] = prePwmDuty[0];
hajime_nntc 0:4ed9b2004dd0 46 prePwmDuty[3] = prePwmDuty[1];
hajime_nntc 0:4ed9b2004dd0 47 }
hajime_nntc 0:4ed9b2004dd0 48 for(i = 0; i < 4; i++) {
hajime_nntc 0:4ed9b2004dd0 49 // 値を-1.0~1.0の範囲だからその範囲に収める
hajime_nntc 0:4ed9b2004dd0 50 if(prePwmDuty[i] < -1.0) {
hajime_nntc 0:4ed9b2004dd0 51 prePwmDuty[i] = -1.0;
hajime_nntc 0:4ed9b2004dd0 52 } else if(prePwmDuty[i] > 1.0) {
hajime_nntc 0:4ed9b2004dd0 53 prePwmDuty[i] = 1.0;
hajime_nntc 0:4ed9b2004dd0 54 }
hajime_nntc 0:4ed9b2004dd0 55 PwmDuty[i] = (fabs(prePwmDuty[i] + 1.0)) / 2;
hajime_nntc 0:4ed9b2004dd0 56 }
hajime_nntc 0:4ed9b2004dd0 57 PwmDuty[2] = 1 - PwmDuty[2];
hajime_nntc 0:4ed9b2004dd0 58 PwmDuty[3] = 1 - PwmDuty[3];
hajime_nntc 0:4ed9b2004dd0 59 MDFR[0] = PwmDuty[0]*255;
hajime_nntc 0:4ed9b2004dd0 60 MDRR[0] = PwmDuty[1]*255;
hajime_nntc 0:4ed9b2004dd0 61 MDRL[0] = PwmDuty[2]*255;
hajime_nntc 0:4ed9b2004dd0 62 MDFL[0] = PwmDuty[3]*255;
hajime_nntc 0:4ed9b2004dd0 63 i2cVAL += i2c.write(addr[0], MDFR, 2, false);
hajime_nntc 0:4ed9b2004dd0 64 i2cVAL += i2c.write(addr[1], MDRR, 2, false);
hajime_nntc 0:4ed9b2004dd0 65 i2cVAL += i2c.write(addr[2], MDRL, 2, false);
hajime_nntc 0:4ed9b2004dd0 66 i2cVAL += i2c.write(addr[3], MDFL, 2, false);
hajime_nntc 0:4ed9b2004dd0 67 if(i2cVAL) {
hajime_nntc 0:4ed9b2004dd0 68 leds[2] = 1;
hajime_nntc 0:4ed9b2004dd0 69 } else {
hajime_nntc 0:4ed9b2004dd0 70 leds[2] = 0;
hajime_nntc 0:4ed9b2004dd0 71 }
hajime_nntc 0:4ed9b2004dd0 72 printf("%d,%d,%d,%d,%d\r\n", dig, error_val, MDFR[0], MDRL[0], data[4]);
hajime_nntc 0:4ed9b2004dd0 73 }
hajime_nntc 0:4ed9b2004dd0 74 }