春ロボ1班(元F3RC4班+)
3/6
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 9:337fe0747940, committed 2019-03-06
- Comitter:
- yuki0701
- Date:
- Wed Mar 06 03:51:08 2019 +0000
- Parent:
- 8:4bdfac5b52e1
- Commit message:
- a
Changed in this revision
diff -r 4bdfac5b52e1 -r 337fe0747940 can/can.cpp
--- a/can/can.cpp Tue Mar 05 04:29:49 2019 +0000
+++ b/can/can.cpp Wed Mar 06 03:51:08 2019 +0000
@@ -11,17 +11,16 @@
DigitalOut cansend_led(LED1); //canread -> on
DigitalOut canread_led(LED2); //cansend -> on
-char t1[1]; //動作番号（送信する値（char型））
-int t1_r, T1; //動作番号（受け取った値、送信する値（int型））
+int t1_r=0, T1=0; //動作番号（受け取った値、送信する値（int型））
void can_readsend()
{
- t1[0] = T1;
- printf("t1[0] = %d\n\r",t1[0]);
+ can_ashileddata[1] = T1; //動作番号（id節約のため、can_ashileddata[]と一緒に送る）
+ //printf("t1[0] = %d\n\r",t1[0]);
CANMessage msg;
-
- if(can1.write(CANMessage(9,t1,4))) {
+
+ if(can1.write(CANMessage(4,can_ashileddata,2))) { //IDを7にして送信
cansend_led = 1;
} else {
cansend_led = 0;
@@ -58,8 +57,8 @@
// debug_printf("usw_data4 = %f\n\r",usw_data4);
}
- if(msg.id == 8) {
- t1_r = msg.data[0];
+ if(msg.id == 1) {
+ t1_r = msg.data[3];
//printf("t1_r = %d\n\r",t1_r);
}
@@ -67,13 +66,7 @@
canread_led = 0;
}
- if(can1.write(CANMessage(7,can_ashileddata,1))) { //IDを7にして送信
- cansend_led = 1;
- } else {
- cansend_led = 0;
- }
-
- if(t1_r > T1) {
+ if(t1_r > T1){
T1 = t1_r;
}
diff -r 4bdfac5b52e1 -r 337fe0747940 main.cpp
--- a/main.cpp Tue Mar 05 04:29:49 2019 +0000
+++ b/main.cpp Wed Mar 06 03:51:08 2019 +0000
@@ -41,18 +41,19 @@
#ifdef HARUROBO_TEST_MODE
T1 = 0;
printf("start\n\r");
- printf("T1 = %d\n\r",T1);
+ //printf("T1 = %d\n\r",T1);
if(T1 == 0) {
//スタート位置からみかんの木まで移動
- printf("t = 0");
+ printf("T1 = 0\n\r");
T1++;
//printf("t = %d\n\r",T1);
}
if(T1 == 1) {
while(1) {
- //printf("wait\n\r");
+ printf("T1 = 1\n\r");
+ wait(0.5);
if(T1 == 2) {
break;
}
@@ -62,13 +63,15 @@
if(T1 == 2) {
//みかんの木から三宝置き場まで移動
- printf("t = 2");
+ printf("T1 = 2\n\r");
T1++;
}
if(T1 == 3) {
while(1) {
+ printf("T1 = 3\n\r");
if(T1 == 4) {
+
break;
}
}
@@ -118,19 +121,20 @@
if(go_waitmode == 0) {
-
+
//can_start();
gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0);
purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
- purecurve(8,1,1,-2317,2500,-1610,2000,9,1000,5,0.1,10,0.1,600,0);
- gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0);
+ purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
+ gogo_straight(1,1,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
MaxonControl(0,0,0,0);
- set_cond(2,0,-1243,1,800);
- pos_correction(-1610,1250,0,1,0);
+ set_cond(2,0,-1243,1,1076);
+ pos_correction(-1672,1400,0,0,0);
+ enc_correction(1,1);
wait(0.5);
- gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0);
- purecurve(3,1,1,-1610,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
+ gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
+ purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90);
gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90);
@@ -140,7 +144,8 @@
set_cond(0,0,-2462,0,0);
gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
MaxonControl(0,0,0,0);
- pos_correction(-2850,4500,-90,1,1);
+ pos_correction(-2850,4500,-90,0,1);
+ enc_correction(1,0);
wait(0.5);
gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
@@ -151,19 +156,20 @@
set_cond(2,1,-3500,1,6324);
gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
MaxonControl(0,0,0,0);
- pos_correction(-2700,6647,-90,1,1);
+ pos_correction(-2700,6647,-90,0,0);
+ enc_correction(1,1);
wait(0.5);
gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90);
gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90);
- purecurve(8,1,1,-1000,6647,-350,6000,9,1000,5,0.1,10,0.1,600,-180);
- gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180);
+ purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
+ gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
set_cond(2,1,1243,1,4000);
- gogo_straight(0,0,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
+ gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
MaxonControl(0,0,0,0);
- pos_correction(-400,4500,-180,1,1);
+ pos_correction(-519,4500,-180,0,0);
MaxonControl(0,0,0,0);
-
+
go_waitmode = 1;
} else if(go_waitmode == 1) {
diff -r 4bdfac5b52e1 -r 337fe0747940 movement/movement.cpp
--- a/movement/movement.cpp Tue Mar 05 04:29:49 2019 +0000
+++ b/movement/movement.cpp Wed Mar 06 03:51:08 2019 +0000
@@ -11,7 +11,7 @@
#define PI 3.141592
-char can_ashileddata[1]= {0};
+char can_ashileddata[2]= {0};
int can_ashileddata0_0,can_ashileddata0_1,can_ashileddata0_2,can_ashileddata0_3;
Ec EC1(p22,p21,NC,500,0.05);
@@ -172,7 +172,7 @@
//x_base,y_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標/x軸平行の壁のy座標)
double R1=240,R2=240,R3=240,R4=240; //機体の中心から各超音波センサーが付いている面までの距離
- double D1=30,D2=30,D3=30,D4=30; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離
+ double D1=42,D2=0,D3=0,D4=0; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離
now_angle=gyro.getAngle(); //ジャイロの値読み込み
@@ -284,6 +284,18 @@
}*/
}
+void enc_correction(int x_select,int y_select){ //エンコーダの座標を超音波センサの座標で上書き
+//x_select,y_select → (0:上書きしない/1:上書きする)
+
+ if(x_select == 1){
+ info.nowX.enc = info.nowX.usw;
+ }
+ if(y_select == 1){
+ info.nowY.enc = info.nowY.usw;
+ }
+
+}
+
//ここからそれぞれのプログラム/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
//ジャイロの出力は角度だが三角関数はラジアンとして計算する
diff -r 4bdfac5b52e1 -r 337fe0747940 movement/movement.h
--- a/movement/movement.h Tue Mar 05 04:29:49 2019 +0000
+++ b/movement/movement.h Wed Mar 06 03:51:08 2019 +0000
@@ -1,7 +1,7 @@
#ifndef HARUROBO2019_MOVEMENT
#define HARUROBO2019_MOVEMENT
-extern char can_ashileddata[1];
+extern char can_ashileddata[2];
extern int16_t m1,m2,m3,m4;
@@ -25,6 +25,8 @@
void calc_xy(double tgt_angle, double u, double v);
+void enc_correction(int x_select,int y_select);
+
void purecurve(int type,double u, double v, //正面を変えずに円弧or楕円を描いて曲がる
double point_x1,double point_y1,
double point_x2,double point_y2,