shiyuu oguro / Mbed 2 deprecated Daisen0

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

Dependencies:   HMC6352 mbed

Fork of rcj2015_CatPot_MotorTest by Fumiya Fujisawa

main.cpp@2:89bdaf269693, 2016-03-07 (annotated)

Committer:
shiyuu
Date:
Mon Mar 07 07:30:55 2016 +0000
Revision:
2:89bdaf269693
Parent:
1:e8b1e591f61e
Child:
3:69ce73ed70a7
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shiyuu 2:89bdaf269693 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 2:89bdaf269693 10 #define LIMIT 1
shiyuu 2:89bdaf269693 11 #define TRIALS 1
shiyuu 1:e8b1e591f61e 12 #define ZERO 0
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 1:e8b1e591f61e 34 pwm[0] = ( sin((see - 60.0)* PAI/180.0)*POWER )- direction * 0.7;
shiyuu 1:e8b1e591f61e 35 pwm[1] = ( sin((see - 300.0)* PAI/180.0)*POWER )- direction * 0.7;
shiyuu 1:e8b1e591f61e 36 pwm[2] = ( sin((see - 180.0)* PAI/180.0)*POWER )- direction * 0.7;
shiyuu 1:e8b1e591f61e 37 pwm[3] = 0;
shiyuu 1:e8b1e591f61e 38 pwm[0] *= LIMIT;
shiyuu 1:e8b1e591f61e 39 pwm[1] *= LIMIT;
shiyuu 1:e8b1e591f61e 40 pwm[2] *= LIMIT;
shiyuu 1:e8b1e591f61e 41 //pc.printf("%d %d %d\n",speed[0],speed[1],speed[2]);
ryuna 0:617b63d4a532 42 for(i = 0; i < 4; i++){
ryuna 0:617b63d4a532 43 if(pwm[i] > 100){
ryuna 0:617b63d4a532 44 pwm[i] = 100;
ryuna 0:617b63d4a532 45 } else if(pwm[i] < -100){
ryuna 0:617b63d4a532 46 pwm[i] = -100;
ryuna 0:617b63d4a532 47 }
ryuna 0:617b63d4a532 48 speed[i] = pwm[i];
ryuna 0:617b63d4a532 49 }
ryuna 0:617b63d4a532 50 }
shiyuu 1:e8b1e591f61e 51
shiyuu 1:e8b1e591f61e 52 int diffKun(int sampleC,int first){
shiyuu 1:e8b1e591f61e 53 int diff=0;
shiyuu 1:e8b1e591f61e 54 diff = sampleC + 540 - first;//0~180度の範囲で動かしたかった
shiyuu 1:e8b1e591f61e 55 diff %= 360;
shiyuu 1:e8b1e591f61e 56 diff -= 180;
shiyuu 1:e8b1e591f61e 57 return diff;
shiyuu 1:e8b1e591f61e 58 }
shiyuu 1:e8b1e591f61e 59
ryuna 0:617b63d4a532 60 int main() {
shiyuu 2:89bdaf269693 61 int seeta=0,diff=0,getFirst=0,min1=0,min2=1,max1=359,max2=358;
shiyuu 1:e8b1e591f61e 62 int sampleC[TRIALS] = {0},getCompass[TRIALS] = {0};
shiyuu 2:89bdaf269693 63 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);//最初のSetting
ryuna 0:617b63d4a532 64
shiyuu 2:89bdaf269693 65 //
shiyuu 2:89bdaf269693 66 getFirst = ( compass.sample() / 10.0 );
shiyuu 2:89bdaf269693 67
shiyuu 1:e8b1e591f61e 68 seeta = 0;
shiyuu 1:e8b1e591f61e 69 motor.baud(115200); //ボーレート設定
shiyuu 1:e8b1e591f61e 70 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 モーター番号(1~6) + ForR(前進後進) + パワー(0~100)
shiyuu 1:e8b1e591f61e 71 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
shiyuu 1:e8b1e591f61e 72
shiyuu 1:e8b1e591f61e 73 //LEDは目印
shiyuu 1:e8b1e591f61e 74 m_led = 0x01;
shiyuu 1:e8b1e591f61e 75 wait(1);
shiyuu 1:e8b1e591f61e 76 m_led = 0x03;
shiyuu 1:e8b1e591f61e 77 wait(1);
shiyuu 1:e8b1e591f61e 78 m_led = 0x07;
shiyuu 1:e8b1e591f61e 79 wait(1);
shiyuu 1:e8b1e591f61e 80 m_led = 0x0F;
shiyuu 1:e8b1e591f61e 81 //
shiyuu 1:e8b1e591f61e 82
shiyuu 1:e8b1e591f61e 83 while(1){
shiyuu 2:89bdaf269693 84 //
shiyuu 2:89bdaf269693 85 /*for(j=0;j<TRIALS;j++){
shiyuu 1:e8b1e591f61e 86 getCompass[j] = ( compass.sample() / 10.0 );
shiyuu 2:89bdaf269693 87 //pc.printf("%d\n",getCompass[j]);
shiyuu 2:89bdaf269693 88 getCompass[j] = diffKun(getCompass[j],getFirst);
shiyuu 2:89bdaf269693 89 if((getCompass[j]>359)&&(j<0)) getCompass[j] = getCompass[j-1];
shiyuu 2:89bdaf269693 90 diff += getCompass[j];
shiyuu 1:e8b1e591f61e 91 }
shiyuu 2:89bdaf269693 92 diff /= TRIALS;
shiyuu 2:89bdaf269693 93 diff = diffKun(diff,getFirst);
shiyuu 2:89bdaf269693 94 pc.printf("%d\n",diff);*/
shiyuu 2:89bdaf269693 95 //
shiyuu 2:89bdaf269693 96 getCompass[0] = ( compass.sample() / 10.0 );
shiyuu 2:89bdaf269693 97 diff = diffKun(getCompass[0],getFirst);
shiyuu 2:89bdaf269693 98
shiyuu 1:e8b1e591f61e 99 move(seeta,diff);
shiyuu 2:89bdaf269693 100 pc.printf("%d %d %d",getFirst,getCompass[0],diff);
shiyuu 2:89bdaf269693 101 pc.printf(" %d %d %d\n",speed[0],speed[1],speed[2]);
shiyuu 2:89bdaf269693 102 //wait(0.01);
shiyuu 1:e8b1e591f61e 103 }
shiyuu 1:e8b1e591f61e 104 /*wait(1);
shiyuu 1:e8b1e591f61e 105 speed[0] = 0;
shiyuu 1:e8b1e591f61e 106 speed[1] = 0;
shiyuu 1:e8b1e591f61e 107 speed[2] = 0;*/
shiyuu 1:e8b1e591f61e 108 while(1);
ryuna 0:617b63d4a532 109 }