shiyuu oguro / Mbed 2 deprecated Daisen0

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

Dependencies:   HMC6352 mbed

Fork of rcj2015_CatPot_MotorTest by Fumiya Fujisawa

main.cpp@1:e8b1e591f61e, 2016-03-07 (annotated)

Committer:
shiyuu
Date:
Mon Mar 07 06:04:16 2016 +0000
Revision:
1:e8b1e591f61e
Parent:
0:617b63d4a532
Child:
2:89bdaf269693
a

Who changed what in which revision?

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