shiyuu oguro / Mbed 2 deprecated Daisen0

コントロールの前段階。姿勢制御の実験中。

Dependencies:   HMC6352 mbed

Fork of rcj2015_CatPot_MotorTest by Fumiya Fujisawa

main.cpp@3:69ce73ed70a7, 2016-03-07 (annotated)

Committer:
shiyuu
Date:
Mon Mar 07 09:08:05 2016 +0000
Revision:
3:69ce73ed70a7
Parent:
2:89bdaf269693 
?????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shiyuu 3:69ce73ed70a7 1 /* 取り敢えずこれで。 */
shiyuu 2:89bdaf269693 2
ryuna 0:617b63d4a532 3 #include "mbed.h"
shiyuu 1:e8b1e591f61e 4 #include "HMC6352.h"
ryuna 0:617b63d4a532 5 #include <math.h>
ryuna 0:617b63d4a532 6 #include <sstream>
ryuna 0:617b63d4a532 7
shiyuu 1:e8b1e591f61e 8 #define PAI 3.1415926535897932384626
shiyuu 1:e8b1e591f61e 9 #define POWER 0
shiyuu 3:69ce73ed70a7 10 #define PROPORTION 0.7
shiyuu 3:69ce73ed70a7 11 #define INSTANCE 1.05
shiyuu 2:89bdaf269693 12 #define LIMIT 1
ryuna 0:617b63d4a532 13
shiyuu 1:e8b1e591f61e 14 BusOut m_led(LED1,LED2,LED3,LED4);
shiyuu 1:e8b1e591f61e 15 //DigitalOut myled(LED1);
shiyuu 1:e8b1e591f61e 16
shiyuu 1:e8b1e591f61e 17 Serial pc(USBTX,USBRX);
shiyuu 1:e8b1e591f61e 18 HMC6352 compass(p9,p10);
ryuna 0:617b63d4a532 19 Serial motor(p13,p14);//tx,rx
ryuna 0:617b63d4a532 20 extern string StringFIN;
ryuna 0:617b63d4a532 21 extern void array(int,int,int,int);
ryuna 0:617b63d4a532 22 int speed[4] = {0};
shiyuu 1:e8b1e591f61e 23 int x = 0, y = 0,i=0,j=0;
ryuna 0:617b63d4a532 24
ryuna 0:617b63d4a532 25 //通信（モータ用）
ryuna 0:617b63d4a532 26 void tx_motor(){
ryuna 0:617b63d4a532 27 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 0:617b63d4a532 28 motor.printf("%s",StringFIN.c_str());
ryuna 0:617b63d4a532 29 }
ryuna 0:617b63d4a532 30
shiyuu 1:e8b1e591f61e 31 void move(double see,int direction){
ryuna 0:617b63d4a532 32 double pwm[4] = {0};
ryuna 0:617b63d4a532 33
shiyuu 3:69ce73ed70a7 34 pwm[0] = ( sin((see - 60.0)* PAI/180.0)*POWER )- direction * PROPORTION * LIMIT;
shiyuu 3:69ce73ed70a7 35 pwm[1] = ( sin((see - 300.0)* PAI/180.0)*POWER )- direction * PROPORTION * LIMIT;
shiyuu 3:69ce73ed70a7 36 pwm[2] = ( sin((see - 180.0)* PAI/180.0)*POWER )- direction * PROPORTION * LIMIT;
shiyuu 1:e8b1e591f61e 37 pwm[3] = 0;
shiyuu 3:69ce73ed70a7 38 if(direction<0){
shiyuu 3:69ce73ed70a7 39 pwm[0] *= INSTANCE;
shiyuu 3:69ce73ed70a7 40 pwm[1] *= INSTANCE;
shiyuu 3:69ce73ed70a7 41 pwm[2] *= INSTANCE;
shiyuu 3:69ce73ed70a7 42 }
shiyuu 1:e8b1e591f61e 43 //pc.printf("%d %d %d\n",speed[0],speed[1],speed[2]);
ryuna 0:617b63d4a532 44 for(i = 0; i < 4; i++){
ryuna 0:617b63d4a532 45 if(pwm[i] > 100){
ryuna 0:617b63d4a532 46 pwm[i] = 100;
ryuna 0:617b63d4a532 47 } else if(pwm[i] < -100){
ryuna 0:617b63d4a532 48 pwm[i] = -100;
ryuna 0:617b63d4a532 49 }
ryuna 0:617b63d4a532 50 speed[i] = pwm[i];
ryuna 0:617b63d4a532 51 }
ryuna 0:617b63d4a532 52 }
shiyuu 1:e8b1e591f61e 53
shiyuu 1:e8b1e591f61e 54 int diffKun(int sampleC,int first){
shiyuu 1:e8b1e591f61e 55 int diff=0;
shiyuu 1:e8b1e591f61e 56 diff = sampleC + 540 - first;//0~180度の範囲で動かしたかった
shiyuu 1:e8b1e591f61e 57 diff %= 360;
shiyuu 1:e8b1e591f61e 58 diff -= 180;
shiyuu 1:e8b1e591f61e 59 return diff;
shiyuu 1:e8b1e591f61e 60 }
shiyuu 1:e8b1e591f61e 61
ryuna 0:617b63d4a532 62 int main() {
shiyuu 2:89bdaf269693 63 int seeta=0,diff=0,getFirst=0,min1=0,min2=1,max1=359,max2=358;
shiyuu 3:69ce73ed70a7 64 int getCompass=0;
shiyuu 2:89bdaf269693 65 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);//最初のSetting
ryuna 0:617b63d4a532 66
shiyuu 2:89bdaf269693 67 //
shiyuu 2:89bdaf269693 68 getFirst = ( compass.sample() / 10.0 );
shiyuu 2:89bdaf269693 69
shiyuu 1:e8b1e591f61e 70 seeta = 0;
shiyuu 1:e8b1e591f61e 71 motor.baud(115200); //ボーレート設定
shiyuu 1:e8b1e591f61e 72 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 モーター番号(1~6) + ForR(前進後進) + パワー(0~100)
shiyuu 1:e8b1e591f61e 73 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
shiyuu 1:e8b1e591f61e 74
shiyuu 1:e8b1e591f61e 75 //LEDは目印
shiyuu 1:e8b1e591f61e 76 m_led = 0x01;
shiyuu 1:e8b1e591f61e 77 wait(1);
shiyuu 1:e8b1e591f61e 78 m_led = 0x03;
shiyuu 1:e8b1e591f61e 79 wait(1);
shiyuu 1:e8b1e591f61e 80 m_led = 0x07;
shiyuu 1:e8b1e591f61e 81 wait(1);
shiyuu 1:e8b1e591f61e 82 m_led = 0x0F;
shiyuu 1:e8b1e591f61e 83 //
shiyuu 1:e8b1e591f61e 84
shiyuu 1:e8b1e591f61e 85 while(1){
shiyuu 3:69ce73ed70a7 86 getCompass = ( compass.sample() / 10.0 );
shiyuu 3:69ce73ed70a7 87 diff = diffKun(getCompass,getFirst);
shiyuu 2:89bdaf269693 88
shiyuu 1:e8b1e591f61e 89 move(seeta,diff);
shiyuu 3:69ce73ed70a7 90 pc.printf("%d %d %d",getFirst,getCompass,diff);
shiyuu 2:89bdaf269693 91 pc.printf(" %d %d %d\n",speed[0],speed[1],speed[2]);
shiyuu 2:89bdaf269693 92 //wait(0.01);
shiyuu 1:e8b1e591f61e 93 }
ryuna 0:617b63d4a532 94 }