robostep8th
/
harurobo_ashi_com7
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
main.cpp
- Committer:
- yuki0701
- Date:
- 2019-03-08
- Revision:
- 10:cf5c159e729a
- Parent:
- 9:63924280272d
- Child:
- 11:8eda88c96bb0
File content as of revision 10:cf5c159e729a:
#include "EC.h"
#include "R1370P.h"
#include "move4wheel.h"
#include "mbed.h"
#include "math.h"
#include "PathFollowing.h"
#include "movement.h"
#include "maxonsetting.h"
#include "manual.h"
#include "can.h"
#define PI 3.141592
//#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
#define HARUROBO_TEST_MODE
//#define HARUROBO_MAIN_MODE
int go_waitmode = 0;
//-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
//DigitalOut cansend_led(LED1); //canread -> on //can.cpp
//DigitalOut canread_led(LED2); //cansend -> on //can.cpp
//DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
//////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
int main()
{
UserLoopSetting_maxon();
UserLoopSetting_sensor();
UserLoopSetting_can();
#ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
printf("ソースファイル名 : %s¥n", __FILE__);
printf("作成日付 : %s¥n", __DATE__);
printf("作成時刻 : %s¥n", __TIME__);
#endif
#ifdef HARUROBO_TEST_MODE
id1_value[0] = 1;
//while(1);
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,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
set_cond(2,0,-1050,1,1076);
gogo_straight(0,0,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
MaxonControl(0,0,0,0);
pos_correction(-1672,1400,0,0,0);
enc_correction(1,1);
wait(0.5);
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); //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);
MaxonControl(0,0,0,0);
pos_correction(-2962,4500,-90,1,1);
set_cond(2,0,-2462,0,6000);
gogo_straight(0,0,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
MaxonControl(0,0,0,0);
pos_correction(-2850,4500,-90,0,0);
enc_correction(1,1);
wait(0.5);
gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90);
purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90);
purecurve(3,1,1,-1700,6000,-2257,6550,9,1000,5,0.1,10,0.1,800,-90);
gogo_straight(1,1,-2257,6550,-2500,6550,1000,1000,5,0.1,10,0.1,800,-90);
set_cond(2,1,-3500,1,6050);
gogo_straight(0,0,-2500,6550,-2700,6600,1000,200,5,0.1,10,0.1,800,-90);
MaxonControl(0,0,0,0);
pos_correction(-2700,6600,-90,0,0);
enc_correction(1,1);
wait(0.5);
gogo_straight(1,1,-2700,6600,-2500,6550,200,1000,5,0.1,10,0.1,800,-90);
gogo_straight(1,1,-2500,6550,-1300,6550,1000,1000,5,0.1,10,0.1,800,-90);
gogo_straight(1,1,-1300,6550,-1000,6500,1000,1000,5,0.1,10,0.1,800,-90);
purecurve(8,1,1,-1000,6550,-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,-1000,1,4000);
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(-519,4500,-180,0,0);
enc_correction(1,1);
MaxonControl(0,0,0,0);
//以下自動で初期位置に戻るプログラム
wait(3);
gogo_straight(1,1,-519,4500,-519,5000,200,1000,5,0.1,10,0.1,600,-180);
gogo_straight(1,1,-519,5000,-519,6000,1000,1000,5,0.1,10,0.1,600,-180);
gogo_straight(1,1,-519,6000,-519,6600,1000,200,5,0.1,10,0.1,600,-180);
MaxonControl(0,0,0,0);
set_cond(2,0,0,0,7000);
pos_correction(-519,6600,-180,0,0);
enc_correction(1,1);
gogo_straight(1,1,-519,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180);
purecurve(5,1,1,-1000,6600,-1672,6000,9,1000,5,0.1,10,0.1,600,-180);
purecurve(6,1,1,-1672,6000,-2462,5500,9,1000,5,0.1,10,0.1,600,-180);
gogo_straight(1,1,-2462,5500,-2962,5500,1000,200,5,0.1,10,0.1,600,-180);
MaxonControl(0,0,0,0);
set_cond(2,1,-3500,0,6000);
pos_correction(-2962,5500,-180,0,0);
enc_correction(1,1);
gogo_straight(1,1,-2962,5500,-2962,4700,200,1000,5,0.1,10,0.1,600,-90);
gogo_straight(1,1,-2962,4700,-2962,4000,1000,1000,5,0.1,10,0.1,600,0);
gogo_straight(1,1,-2962,4000,-2962,3500,1000,200,5,0.1,10,0.1,600,0);
MaxonControl(0,0,0,0);
set_cond(2,1,-3500,1,2000);
pos_correction(-3000,3500,0,0,0);
enc_correction(1,1);
MaxonControl(0,0,0,0);
#endif
#ifdef HARUROBO_MAIN_MODE
while(1) {
switch(id1_value[0]) {
//-----auto mode----------------------------------------------------------------------------------------------------------------------//
case 1:
switch(id1_value[6]) {
case 0:
//-----right mode-------------------------------------------------------------------------------------------------------------//
if(T1 == 0) {
//スタート位置からみかんの木まで移動
wait(0.5);
T1++;
}
if(T1 == 1) {
while(1) {
wait(0.5);
if(T1 == 2) {
break;
}
}
}
if(T1 == 2) {
//みかんの木から三宝置き場まで移動
T1++;
}
if(T1 == 3) {
while(1) {
wait(0.5);
if(T1 == 4) {
break;
}
}
}
if(T1 == 4) {
//三宝置き場からりんごの木まで移動
wait(0.5);
T1++;
}
if(T1 == 5) {
while(1) {
wait(0.5);
if(T1 == 6) {
break;
}
}
}
if(T1 == 6) {
//りんごの木からお供え台まで移動
wait(0.5);
T1++;
}
break;
case 1:
//-----left mode--------------------------------------------------------------------------------------------------------------//
break;
}
//-----wait mode----------------------------------------------------------------------------------------------------------------------//
case 0:
calc_xy(0,1,1);
ashi_led();
MaxonControl(0,0,0,0);
go_waitmode = 0;
break;
//-----manual mode--------------------------------------------------------------------------------------------------------------------//
case 2:
ManualOut(250,100,500,200);
go_waitmode = 0;
break;
}
//------------------------------------------------------------------------------------------------------------------------------------//
}
#endif
}