shiyuu oguro / Mbed 2 deprecated Daisen0

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

Dependencies:   HMC6352 mbed

Fork of rcj2015_CatPot_MotorTest by Fumiya Fujisawa

main.cpp@0:617b63d4a532, 2016-02-27 (annotated)

Committer:
ryuna
Date:
Sat Feb 27 06:01:52 2016 +0000
Revision:
0:617b63d4a532
Child:
1:e8b1e591f61e
???????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:617b63d4a532 1 #include "mbed.h"
ryuna 0:617b63d4a532 2 #include <math.h>
ryuna 0:617b63d4a532 3 #include <sstream>
ryuna 0:617b63d4a532 4
ryuna 0:617b63d4a532 5 DigitalOut myled(LED1);
ryuna 0:617b63d4a532 6
ryuna 0:617b63d4a532 7 Serial motor(p13,p14);//tx,rx
ryuna 0:617b63d4a532 8 extern string StringFIN;
ryuna 0:617b63d4a532 9 extern void array(int,int,int,int);
ryuna 0:617b63d4a532 10
ryuna 0:617b63d4a532 11 int speed[4] = {0};
ryuna 0:617b63d4a532 12 int x = 0, y = 0,i;
ryuna 0:617b63d4a532 13
ryuna 0:617b63d4a532 14
ryuna 0:617b63d4a532 15 //通信（モータ用）
ryuna 0:617b63d4a532 16 void tx_motor(){
ryuna 0:617b63d4a532 17 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 0:617b63d4a532 18 motor.printf("%s",StringFIN.c_str());
ryuna 0:617b63d4a532 19 }
ryuna 0:617b63d4a532 20
ryuna 0:617b63d4a532 21 void move(int vx, int vy){
ryuna 0:617b63d4a532 22 double pwm[4] = {0};
ryuna 0:617b63d4a532 23
ryuna 0:617b63d4a532 24 pwm[0] = 0;
ryuna 0:617b63d4a532 25 pwm[1] = -100;//(double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy));
ryuna 0:617b63d4a532 26 pwm[2] = //(double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy));
ryuna 0:617b63d4a532 27 pwm[3] = 100;//(double)((vx));
ryuna 0:617b63d4a532 28 for(i = 0; i < 4; i++){
ryuna 0:617b63d4a532 29 if(pwm[i] > 100){
ryuna 0:617b63d4a532 30 pwm[i] = 100;
ryuna 0:617b63d4a532 31 } else if(pwm[i] < -100){
ryuna 0:617b63d4a532 32 pwm[i] = -100;
ryuna 0:617b63d4a532 33 }
ryuna 0:617b63d4a532 34 speed[i] = pwm[i];
ryuna 0:617b63d4a532 35 }
ryuna 0:617b63d4a532 36 }
ryuna 0:617b63d4a532 37 int main() {
ryuna 0:617b63d4a532 38
ryuna 0:617b63d4a532 39 wait(1);
ryuna 0:617b63d4a532 40 motor.baud(115200); //ボーレート設定
ryuna 0:617b63d4a532 41 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 0:617b63d4a532 42 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 0:617b63d4a532 43
ryuna 0:617b63d4a532 44
ryuna 0:617b63d4a532 45 x = -10;
ryuna 0:617b63d4a532 46 y = 10;
ryuna 0:617b63d4a532 47 move(x,y);
ryuna 0:617b63d4a532 48 while(1) {
ryuna 0:617b63d4a532 49 wait(1);
ryuna 0:617b63d4a532 50 }
ryuna 0:617b63d4a532 51 }