riko-ten
これが本体 スイッチ１→前後移動＋サーボ スイッチ２→八の字移動 なお動作未検
Revision 0:a92631a4021d, committed 2018-11-03
- Comitter:
- aoikoizumi
- Date:
- Sat Nov 03 23:13:31 2018 +0000
- Commit message:
- ?????
; ??????????????
; ???????????
; ???????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/F3RC.lib Sat Nov 03 23:13:31 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/riko-ten/code/F3RC/#dc2a1e81c318
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Nov 03 23:13:31 2018 +0000
@@ -0,0 +1,206 @@
+#include "mbed.h"
+#include "F3rc.h"
+
+int main()
+{
+ gyro.initialize(); //main関数の最初に一度だけ実行
+ gyro.acc_offset(); //やってもやらなくてもいい
+
+ printf("start\r\n");
+ motor_tick.attach(&calOmega,0.05);
+ motorR.setDOconstant(30);
+ motorL.setDOconstant(30);//c
+ motorR.setPDparam(0.1,0);
+ motorL.setPDparam(0.1,0);
+
+
+
+ EC1.setDiameter_mm(48);//sokuteirinnhannkei
+ double getDistance_mm();
+ void reset ();
+ EC1.reset();
+
+ motor_f.period_ms(30);
+ motor_b.period_ms(30);//arm
+
+ while(1) {
+ angle=gyro.getAngle()*(-1); //角度の値を受け取る
+ EC1.getDistance_mm();
+ if(target_R==0) motorR.stop();
+ else motorR.Sc(target_R);
+ if(target_L==0) motorL.stop();
+ else motorL.Sc(target_L);
+
+ new_dist=EC1.getDistance_mm();
+ d_dist=new_dist-old_dist;
+ old_dist=new_dist;
+ double d_x=d_dist*sin(angle*3.1415926535/180);
+ double d_y=d_dist*cos(angle*3.1415926535/180);
+ x=x+d_x;
+ y=y+d_y;
+
+ if(kai>=3) {
+ pc.printf("R=%f L=%f",target_R,target_L);
+ pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega());
+ pc.printf("i=%d\r\n",i);
+ pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
+ pc.printf("x=%f y=%f",x,y);
+ //pc.printf("angle=%f\r\n",angle);
+ //機体の進む方向、右モーターの角速度、左モーターの角速度を表示
+ kai=0;
+ }
+ kai++;
+ if(i==0) {
+ servo.pulsewidth_us(950);
+ if(reset_f.read()==0&&button.read()==0) {
+ wait(0.05);
+ if(reset_f.read()==0&&button.read()==0) {
+ denjiben=1;
+ x=0;
+ y=0;
+ i++;
+ }
+ }
+ if(reset_a.read()==0&&button.read()==0) {
+ wait(0.05);
+ if(reset_a.read()==0&&button.read()==0) {
+ denjiben=1;
+ x=0;
+ y=0;
+ i=21;
+ }
+ }//x,y
+ }
+ if(i==1) {
+ wait(3);
+ servo.pulsewidth_us(950);
+ wait(1);
+ i++;
+ }
+
+ if(i==2) {
+ susumu_y(1,1,1000);
+ }
+ if(i==3) {
+
+ motorR.stop();
+ motorL.stop();
+ servo.pulsewidth_us(2200);
+ wait(1);
+ denjiben=0;
+ wait(1);
+ i++;
+ }
+ if(i==4) {
+ servo.pulsewidth_us(950);
+ wait(1);
+ i++;
+ }
+ if(i==5) {
+ susumu_y(-1,-1,200);
+ }
+ if(i==6) {
+ motorR.stop();
+ motorL.stop();
+ denjiben=1;
+ wait(1);
+ i++;
+ }
+
+ if(i==7) {
+ tgt(0,0);
+ }
+ if(i==10) {
+ t.start();
+ while(t<1) {
+ motor_f=0.82;
+ motor_b=0;
+ }
+ t.stop();
+ t.reset();
+
+ t.start();
+ while(t<1) {
+ motor_f=0;
+ motor_b=0.82;
+ }
+ t.stop();
+ t.reset();
+ motor_f=0;
+ motor_b=0;
+ i++;
+ }
+ if(i==11) {
+ susumu_y(1,1,1000);
+ }
+ if(i==12) {
+ susumu_ang(0.5,1,90);
+ }
+ if(i==13){
+ susumu_xl(1,1,500);
+ }
+ if(i==14){
+ susumu_ang(0.5,1,180);
+ }
+ if(i==15) {
+ susumu_y(1,1,415);
+ }
+ if(i==16) {
+ susumu_ang(0.5,1,270);
+ }
+ if(i==17){
+ susumu_xr(1,1,585);
+ }
+ if(i==18){
+ susumu_ang(0.5,1,360);
+ }
+ if(i==19){
+ i=11;
+ }
+
+ if(i==21) {
+ susumu_y(1,1,300);
+ }
+ if(i==22) {
+ susumu_ang(0.333,1,270);
+ }
+ if(i==23){
+ susumu_xr(1,1,-300);
+ }
+ if(i==24){
+ susumu_ang(1,0.333,0);
+ }
+ if(i==25) {
+ susumu_y(1,1,300);
+ }
+ if(i==26) {
+ susumu_ang(1,0.333,-270);
+ }
+ if(i==27){
+ susumu_xl(1,1,300);
+ }
+ if(i==28){
+ susumu_ang(0.333,1,0);
+ }
+ if(i==29){
+ susumu_y(1,1,0);
+ }
+
+
+
+
+
+
+
+
+ }
+
+
+
+
+
+
+
+
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Nov 03 23:13:31 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file