春ロボ ロケット団
/
Right_F3RC_mar4_2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed Encoder CruizCore_R1370P
Diff: main.cpp
- Revision:
- 0:9c9994747ea7
- Child:
- 1:1cb20fa989c2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Oct 04 12:43:40 2019 +0000
@@ -0,0 +1,374 @@
+#include "mbed.h"
+#include "math.h"
+#include "R1370P.h"
+#include "EC.h"
+#define RESOLUTION 500
+PwmOut TB(PB_8);//Top motor Forward
+PwmOut TF(PC_9);//Top motor Backward
+PwmOut RF(PC_8);//Right motor is okay
+PwmOut RB(PC_6);
+PwmOut LF(PB_13);//Left motor ONLY F is okay
+PwmOut LB(PB_15);
+
+PwmOut servo(PA_15);
+
+DigitalOut led1(LED1);
+DigitalIn button(USER_BUTTON);
+DigitalOut out1(PB_9);
+DigitalIn RandL(PC_12);
+
+Serial pc(USBTX, USBRX); // tx, rx
+R1370P gyro(PA_11,PA_12); // tx, rx
+Ticker ticker;
+
+//Serial pc(USBTX,USBRX);
+Ec1multi EC(PA_8,PA_9,RESOLUTION); //1逓倍用class
+Ec1multi EC2(PB_3,PB_5,RESOLUTION);
+
+void rotation(double angle)
+{
+ double a,b;
+ if( angle > 1) {
+ a = 0.04;
+ b = 0;
+ RF = a;
+ RB = b;
+ LF = a;
+ LB = b;
+ TF = a;
+ TB = b;
+ } else if(angle < -1) {
+ a = 0;
+ b = 0.04;
+ RF = a;
+ RB = b;
+ LF = a;
+ LB = b;
+ TF = a;
+ TB = b;
+ }
+
+}
+
+void go_S(double a)
+{
+ RF = 0;
+ RB = a;
+ LF = a;
+ LB = 0;
+ TF = 0;
+ TB = 0;
+}
+
+void go_B(double a)
+{
+ RF = a;
+ RB = 0;
+ LF = 0;
+ LB = a;
+ TF = 0;
+ TB = 0;
+}
+
+void go_R(double a)
+{
+ TB = a;
+ TF = 0;
+ RF = a/2;
+ RB = 0;
+ LF = a/2;
+ LB = 0;
+}
+
+void go_L(double a)
+{
+ TB = 0;
+ TF = a;
+ RF = 0;
+ RB = a/2;
+ LF = 0;
+ LB = a/2;
+}
+
+int main()
+{
+ TF.period_ms(1);
+ TB.period_ms(1);
+ RF.period_ms(1);
+ RB.period_ms(1);
+ LF.period_ms(1);
+ LB.period_ms(1);
+ out1 = 0;
+ servo.period_ms(20);
+ servo.pulsewidth_us(550);
+ wait(1);
+ int count=0,count2=0;
+ double angle;
+ //ticker.attach(&calcOmega,0.5);
+ gyro.initialize(); //main関数の最初に一度だけ実行
+ gyro.acc_offset(); //やってもやらなくてもいい
+
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f ",angle);
+ pc.printf("\r\n");
+
+ while(count < 200) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f ",angle);
+ pc.printf("first");
+ pc.printf("\r\n");
+ go_R(0.1);
+ rotation(angle);
+ }
+ rotation(angle);
+ while(count2 < 10800) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f ",angle);
+ pc.printf("\r\n");
+ go_S(0.1);
+ rotation(angle);
+ }
+ while(count < 195) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f",angle);
+ pc.printf("\r\n");
+ go_R(0.1);
+ rotation(angle);
+ }
+ go_R(0);
+ while(count > 205) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f",angle);
+ pc.printf("\r\n");
+ go_L(0.1);
+ rotation(angle);
+ }
+ go_L(0);
+ //get (-600,10000)
+ servo.pulsewidth_us(600);
+ wait(0.5);
+ servo.pulsewidth_us(700);
+ wait(0.5);
+ servo.pulsewidth_us(750);
+ wait(0.5);
+ servo.pulsewidth_us(800);
+ wait(0.5);
+ servo.pulsewidth_us(850);
+ wait(0.5);
+ servo.pulsewidth_us(900);
+ wait(0.5);
+ servo.pulsewidth_us(950);
+ wait(0.5);
+ servo.pulsewidth_us(1000);
+ wait(0.5);
+ servo.pulsewidth_us(1050);
+ wait(0.5);
+ servo.pulsewidth_us(1070);
+ wait(0.5);
+ servo.pulsewidth_us(1100);
+ wait(0.5);
+ servo.pulsewidth_us(1180);
+ wait(1);
+ out1 = 1;//catch
+ wait(2);
+ servo.pulsewidth_us(1150);
+ //have caught
+ while(count2 > 200) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f ",angle);
+ pc.printf("\r\n");
+ go_B(0.1);
+ rotation(angle);
+ }
+ while(count < 195) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f",angle);
+ pc.printf("\r\n");
+ go_R(0.1);
+ rotation(angle);
+ }
+ while(count > 205) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f",angle);
+ pc.printf("\r\n");
+ go_L(0.1);
+ rotation(angle);
+ }
+ while(angle > -85) {
+ double a,b;
+ a = 0.05;
+ b = 0;
+ RF = a;
+ RB = b;
+ LF = a;
+ LB = b;
+ TF = a;
+ TB = b;
+ angle=gyro.getAngle();
+ }
+ //now (-700,100)
+ //EC reset
+ pc.printf("1\r\n");
+ while(count2 < 1200) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f ",angle);
+ pc.printf("\r\n");
+ go_S(0.1);
+ }//(0,600)
+ pc.printf("2\r\n");
+ go_S(0);
+ go_R(0);
+ go_L(0);
+ wait(15);
+ pc.printf("3\r\n");
+ while(count2 > 500) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f ",angle);
+ pc.printf("\r\n");
+ go_B(0.1);
+ }
+ go_B(0);
+
+ pc.printf("4\r\n");
+ //now(0,0)
+ while( angle > 1) {
+ double a,b;
+ angle=gyro.getAngle();
+ a = 0.05;
+ b = 0;
+ RF = a;
+ RB = b;
+ LF = a;
+ LB = b;
+ TF = a;
+ TB = b;
+ }
+ while(angle < -1) {
+ double a,b;
+ angle=gyro.getAngle();
+ a = 0;
+ b = 0.05;
+ RF = a;
+ RB = b;
+ LF = a;
+ LB = b;
+ TF = a;
+ TB = b;
+ }
+ //now (0,0)
+ pc.printf("5\r\n");
+ while(count2 < 11800) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f ",angle);
+ pc.printf("\r\n");
+ go_S(0.1);
+ rotation(angle);
+ }//now(0,10000)
+ while(count < 2500) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f",angle);
+ pc.printf("\r\n");
+ go_R(0.1);
+ rotation(angle);
+ }//now(-700,11800)
+ while(count2 < 13000) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f ",angle);
+ pc.printf("\r\n");
+ go_S(0.1);
+ rotation(angle);
+ }//now(-1700,13000)
+ //remove battery
+ wait(3);
+ while(count > 800) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f",angle);
+ pc.printf("\r\n");
+ go_L(0.1);
+ rotation(angle);
+ }
+ go_R(0);
+ go_S(0);
+ go_L(0);
+ //now(0,13000)
+ servo.pulsewidth_us(1100);
+ wait(0.5);
+ servo.pulsewidth_us(1050);
+ wait(0.5);
+ servo.pulsewidth_us(1000);
+ wait(0.5);
+ servo.pulsewidth_us(950);
+ wait(0.5);
+ servo.pulsewidth_us(900);
+ wait(0.5);
+ servo.pulsewidth_us(850);
+ wait(0.5);
+ servo.pulsewidth_us(800);
+ wait(0.5);
+ servo.pulsewidth_us(750);
+ wait(0.5);
+ servo.pulsewidth_us(700);
+ wait(0.5);
+ servo.pulsewidth_us(650);
+ wait(1);
+ servo.pulsewidth_us(600);
+ wait(1);
+ servo.pulsewidth_us(550);
+ wait(1);
+}//now(-800,13000)