春ロボ ロケット団
/
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
main.cpp
- Committer:
- koheim
- Date:
- 2019-10-07
- Revision:
- 3:8b9388a6cabe
- Parent:
- 2:8ab2c4ec07e7
File content as of revision 3:8b9388a6cabe:
#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.05;
b = 0;
RF = RF + a;
RB = RB + b;
LF = LF + a;
LB = LB + b;
TF = TF + a;
TB = TB + b;
} else if(angle < -1) {
a = 0;
b = 0.05;
RF = RF + a;
RB = RB + b;
LF = LF + a;
LB = LB + b;
TF = TF + a;
TB = TB + b;
}
}
void go_S(double a,int x,double b,int count)
//b is value for going right or left
//count is value for staying right potition of x-axis
{
if((x - count)*(x - count) < 144) {//
RF = 0;
RB = a;
LF = a;
LB = 0;
TF = 0;
TB = 0;
} else if(x - count >= 12) {//
RF = 0;
RB = a + b/2;
LF = a;
LB = 0 + b/2;
TF = 0 + b;
TB = 0;
} else if (x - count <= -12) {//
RF = 0 + b/2;
RB = a;
LF = a + b/2;
LB = 0;
TF = 0;
TB = 0 + b;
}
}
void go_B(double a,int x,double b,int count)
// b and count are same as go_S
{
if((x - count)*(x - count) < 144) {//
RF = a;
RB = 0;
LF = 0;
LB = a;
TF = 0;
TB = 0;
} else if(x - count >= 12) {//
RF = a;
RB = 0 + b/2;
LF = 0;
LB = a + b/2;
TF = 0 + b;
TB = 0;
} else if (x - count <= -12) {//
RF = a + b/2;
RB = 0;
LF = 0 + b/2;
LB = a;
TF = 0;
TB = 0 + b;
}
}
void go_R(double a)
{
TB = a;
TF = 0;
RF = a/2;
RB = 0;
LF = a/4;//
LB = 0;
}
void go_L(double a)
{
TB = 0;
TF = a;
RF = 0;
RB = a/2;
LF = 0;
LB = a/2;
}
int main()
{
double a,b;
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.2);
rotation(angle);
}
go_R(0);
rotation(angle);
while(count2 < 10000) {
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.4*/0.5,200,0.1,count);//
rotation(angle);
}
go_L(0);
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.2*/0.3,200,0.1,count);//
rotation(angle);
}
go_L(0);
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);
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.4*/0.5,200,0.1,count);//
rotation(angle);
}
go_L(0);
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_L(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);
while(angle > -85) {
a = 0.05;
b = 0;
RF = a;
RB = b;
LF = a;
LB = b;
TF = a;
TB = b;
angle=gyro.getAngle();
}
go_L(0);
pc.printf("1\r\n");
//now I dont have any idea for this part.
//We should make a code to keep x(count) and y(count2) using math.h like cos() or sin()
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");
a = 0.1;
RF = a;
RB = 0;
LF = 0;
LB = a;
TF = 0;
TB = 0;
}
pc.printf("2\r\n");
go_L(0);
wait(10); //get object
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");
a = 0.1;
RF = 0;
RB = a;
LF = a;
LB = 0;
TF = 0;
TB = 0;
}
go_L(0);
pc.printf("4\r\n");
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;
}
go_L(0);
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;
}
go_L(0);
pc.printf("5\r\n");
int old_count;
int old_count2;
old_count=EC.getCount();
old_count2=EC2.getCount();
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.4,old_count,0.05,count);
rotation(angle);
}
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);
}
old_count=EC.getCount();
old_count2=EC2.getCount();
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.4,count,0.05,count);
rotation(angle);
}
while(count > 600) {
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);
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);
}