倒立振子のプログラムです。ジャイロセンサーの値を積分し、角度を。。モーターの出力を積分して移動距離を算出しています。
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:cb965cafb758
diff -r 000000000000 -r cb965cafb758 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Mar 29 15:00:26 2015 +0000 @@ -0,0 +1,76 @@ +#include "mbed.h" +#define pi 3.141592653589794 +#define waddr 0xD0 +#define raddr 0xD1 +#define kt 0.1 //出力値の各ゲインを調節 +#define ko 0.1 +#define kp 0.1 +#define kd 0.1 +PwmOut mp[4] ={p21,p22,p23,p24}; +I2C sen(p9,p10); +Timer global; +Timer pt; +void motor_cont(char select,double percent)//selectが0の時は左がselectが1の時は右のモータをコントロール。 +{ + select *= 2; + if (percent>0){ + mp[select] = fabs(percent); + mp[select+1] = 0; + }else if (percent <0){ + mp[select] = 0; + mp[select+1] = fabs(percent); + }else{ + mp[select] = 0; + mp[select+1] = 0; + } +} +void sensor_init(){ //センサ計測よーーーーーい + sen.frequency(400000); + char put[2][2]; //送る値の用意 + put[0][0] = 0x6b; + put[0][1] = 0x00; + put[1][0] = 0x37; + put[1][1] = 0x02; + sen.write(waddr,put[0],2); //送信 + sen.write(waddr,put[1],2); +} +void get_gyro(double *x,double *y,double *z,int *time) //ジャイロの値をもってくる +{ + int i; //変数用意 + char tv[6]; + short t[3]; + char put[] ={0x43}; + sen.write(waddr,put,1); //センサに送信 + sen.read(raddr,tv,6); + for(i = 0;i <3;i++){ //xyzそれぞれの値を算出 + t[i] = tv[2*i]<<8+tv[2*i+1]; + } + global.stop(); + *time = global.read_us(); + global.reset(); + global.start(); + *x = t[0]*0.00763; //dag/secに単位換算 + *y = t[1]*0.00763; + *z = t[2]*0.00763; +} + +//------------------main function------------------------------------------------------------------------------------------ +int main() { + global.start(); //プログラムでの経過時間をけいそく(積分などで利用) + sensor_init(); + double x,y,z,x2,theta,omega,power,power2,distance; + int time,time2; + while(1) { + get_gyro(&x,&y,&z,&time); + theta = theta+(x+x2)*time/(2.0*1000000.0); //積分して角度算出 + omega = x; + power = kt*theta + ko*omega +kp*power; + time2 = pt.read_us(); + distance = distance + (power+power2)*time2/(2.0*1000000.0); //移動距離を出力を積分して算出 + power += kd*distance; + pt.reset(); + pt.start(); + x2 = x; + power2 = power; + } +}