春ロボ1班(元F3RC4班+)
/
harurobo_ashi_3_5
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 move4wheel2 EC CruizCore_R1370P
Revision 8:4bdfac5b52e1, committed 2019-03-05
- Comitter:
- yuki0701
- Date:
- Tue Mar 05 04:29:49 2019 +0000
- Parent:
- 7:44ce34007499
- Commit message:
- a
Changed in this revision
--- a/can/can.cpp Sat Mar 02 08:27:05 2019 +0000
+++ b/can/can.cpp Tue Mar 05 04:29:49 2019 +0000
@@ -11,10 +11,22 @@
DigitalOut cansend_led(LED1); //canread -> on
DigitalOut canread_led(LED2); //cansend -> on
+char t1[1]; //動作番号（送信する値（char型））
+int t1_r, T1; //動作番号（受け取った値、送信する値（int型））
+
void can_readsend()
{
+ t1[0] = T1;
+ printf("t1[0] = %d\n\r",t1[0]);
+
CANMessage msg;
+ if(can1.write(CANMessage(9,t1,4))) {
+ cansend_led = 1;
+ } else {
+ cansend_led = 0;
+ }
+
if(can1.read(msg)) {
canread_led = 1;
@@ -28,8 +40,8 @@
id1_value[5] = (msg.data[0]>>2)%2; //left joystick neutral position(0 or 1)
id1_value[6] = (msg.data[0]>>1)%2; //decide right/left(0 or 1)
- debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r"
- ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]);
+ //debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r"
+ // ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]);
}
if(msg.id == 3) {
@@ -43,7 +55,12 @@
//debug_printf("usw_data3 = %f\n\r",usw_data3);
usw_data4 = 0.1 * (short)((msg.data[6]<<8) | msg.data[7]);
- //debug_printf("usw_data4 = %f\n\r",usw_data4);
+ // debug_printf("usw_data4 = %f\n\r",usw_data4);
+ }
+
+ if(msg.id == 8) {
+ t1_r = msg.data[0];
+ //printf("t1_r = %d\n\r",t1_r);
}
} else {
@@ -56,6 +73,10 @@
cansend_led = 0;
}
+ if(t1_r > T1) {
+ T1 = t1_r;
+ }
+
}
void can_start()
{
--- a/can/can.h Sat Mar 02 08:27:05 2019 +0000 +++ b/can/can.h Tue Mar 05 04:29:49 2019 +0000 @@ -1,6 +1,8 @@ #ifndef HARUROBO2019_CAN #define HARUROBO2019_CAN +extern int T1; + void can_readsend(); void can_start();
--- a/main.cpp Sat Mar 02 08:27:05 2019 +0000
+++ b/main.cpp Tue Mar 05 04:29:49 2019 +0000
@@ -12,6 +12,8 @@
#define PI 3.141592
//#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
+#define HARUROBO_TEST_MODE
+//#define HARUROBO_MAIN_MODE
int go_waitmode = 0;
@@ -36,12 +38,74 @@
#endif
+#ifdef HARUROBO_TEST_MODE
+ T1 = 0;
+ printf("start\n\r");
+ printf("T1 = %d\n\r",T1);
+ if(T1 == 0) {
+
+ //スタート位置からみかんの木まで移動
+ printf("t = 0");
+ T1++;
+ //printf("t = %d\n\r",T1);
+ }
+
+ if(T1 == 1) {
+ while(1) {
+ //printf("wait\n\r");
+ if(T1 == 2) {
+ break;
+ }
+ }
+ }
+
+ if(T1 == 2) {
+
+ //みかんの木から三宝置き場まで移動
+ printf("t = 2");
+ T1++;
+ }
+
+ if(T1 == 3) {
+ while(1) {
+ if(T1 == 4) {
+ break;
+ }
+ }
+ }
+
+ if(T1 == 4) {
+
+ //三宝置き場からりんごの木まで移動
+ printf("t = 4");
+ T1++;
+ }
+
+ if(T1 == 5) {
+ while(1) {
+ if(T1 == 6) {
+ break;
+ }
+ }
+ }
+
+ if(T1 == 6) {
+
+ //りんごの木からお供え台まで移動
+ printf("t = 6");
+ T1++;
+ }
+#endif
+
+#ifdef HARUROBO_MAIN_MODE
while(1) {
+ id1_value[0] = 1;
switch(id1_value[0]) {
//-----auto mode----------------------------------------------------------------------------------------------------------------------//
case 1:
+ id1_value[6] = 1;
switch(id1_value[6]) {
case 0:
//-----right mode-------------------------------------------------------------------------------------------------------------//
@@ -50,16 +114,18 @@
case 1:
//-----left mode--------------------------------------------------------------------------------------------------------------//
+ go_waitmode = 0;
if(go_waitmode == 0) {
+
+
//can_start();
- //set_cond(2,1,-700,1,-700);
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);
MaxonControl(0,0,0,0);
- set_cond(2,0,-1243,1,1080);
+ set_cond(2,0,-1243,1,800);
pos_correction(-1610,1250,0,1,0);
wait(0.5);
@@ -72,7 +138,7 @@
pos_correction(-2962,4500,-90,1,1);
set_cond(0,0,-2462,0,0);
- gogo_straight(1,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
+ 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);
wait(0.5);
@@ -83,7 +149,7 @@
purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90);
gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90);
set_cond(2,1,-3500,1,6324);
- gogo_straight(1,1,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
+ 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);
wait(0.5);
@@ -93,10 +159,11 @@
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);
set_cond(2,1,1243,1,4000);
- gogo_straight(1,1,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
+ gogo_straight(0,0,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
MaxonControl(0,0,0,0);
pos_correction(-400,4500,-180,1,1);
-
+ MaxonControl(0,0,0,0);
+
go_waitmode = 1;
} else if(go_waitmode == 1) {
@@ -125,4 +192,6 @@
}
//------------------------------------------------------------------------------------------------------------------------------------//
}
-}
\ No newline at end of file
+#endif
+}
+
--- a/movement/movement.cpp Sat Mar 02 08:27:05 2019 +0000
+++ b/movement/movement.cpp Tue Mar 05 04:29:49 2019 +0000
@@ -216,7 +216,7 @@
}
- } else if(tgt_angle==180) {
+ } else if(tgt_angle==180 || tgt_angle == -180) {
if((xy_type==0 || xy_type==2) && pm_typeX==0) {
info.nowX.usw = x_base - (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
@@ -235,6 +235,26 @@
info.nowY.usw = y_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
}
+ }else if(tgt_angle==-90) {
+ if((xy_type==0 || xy_type==2) && pm_typeX==0) {
+
+ info.nowX.usw = x_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
+
+ } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
+
+ info.nowX.usw = x_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
+
+ }
+ if((xy_type==1 || xy_type==2) && pm_typeY==0) {
+
+ info.nowY.usw = y_base - (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
+
+ } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
+
+ info.nowY.usw = y_base + (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
+
+ }
+
}
}