Ryu Kaz
t6est
Diff: backup.md
- Revision:
- 1:bbf776e6c792
- Parent:
- 0:3dc012104243
- Child:
- 3:8b22783f6bf1
--- a/backup.md Tue Sep 24 13:35:31 2019 +0000
+++ b/backup.md Wed Sep 25 03:57:31 2019 +0000
@@ -4,6 +4,7 @@
#include "foot.h"
#include "rori.h"
#include "peripheral.h"
+
#define UP 0
#define BACK 1
#define RIGHT 2
@@ -18,11 +19,11 @@
//767ってちょっと765っぽいよね
//lets go fiver time!
int sewomukeru[10] = {
- BACK,UP,LEFT,RIGHT,LBACK,RBACK,LUP,RUP,LROLL,RROLL
- };//reverce array
+ BACK,UP,LEFT,RIGHT,LBACK,RBACK,LUP,RUP,LROLL,RROLL//reverce array
+ };
//もはやJava
Serial serial(USBTX,USBRX);
-//RawSerial bt(PF_7,PF_6);
+
DigitalOut outs(PF_6);
DigitalIn ins(PF_7);
DigitalOut outt(D0);
@@ -37,12 +38,13 @@
double gz;
Timer change_rm;
Timer move_tm;
+Timer go_up_tm;
///////////定数たち//////////////
-const float Min_ang = 0.5f;
+const float Min_ang = 0.1f;
const float Max_pwm = 0.2f;
const float Max_pwm_roll = 0.3f;
const float Min_pwm = 0.02f;
-const float Gein = 0.05f;//0.04f
+const float Gein = 0.05f;//0.05f
const int R = 103;//これ直径やんけ
const int Resolution = 1024;
const float RoriGein = 0.001f;
@@ -50,7 +52,7 @@
int point_count = 0;
int array_count = 0;
int C_vari = 0;
-long last_x;
+/////////////////Move/////////////////////////
/*
int points[][3] = {
{RIGHT,3700,-1},
@@ -66,10 +68,12 @@
};
*/
int sheets[] = {
- 900,1700,3500
+ 900,1700,3500//下から:1750,2775,3575
};
+
+//当たるのは1200mm
int nomal_towel[]{
- C+600,C+1100,C+1600
+ 1975,2775,3575//2130,2930,3730
};
int final_towel[]{
C+410,C+1070,C+1730,C+2390
@@ -85,14 +89,13 @@
bool last_move_vertical,last_move_side;
float x_dis,y_dis;
bool flag_finish_move = false;
-//float tempG = RoriGein*0.5f;
+
void setMove(int p0,int p1);
+void get_rorivol();
+
float pwm;
-void rolling();
-void void_read_gyro();
bool activeMove = false;
RORI RX(PF_9,PF_8);
-//RORI RY(PA_4,PB_0);
RORI RY(PA_4_ALT0,PB_0_ALT0);
int abspointx = 0;
@@ -105,9 +108,9 @@
bool point_move = false;
-bool move_lock1 = false;
-bool move_lock2 = false;
bool go_up = false;
+float dis_go_up = 0;
+//////////////////////////////////////////////
int main() {
ins.mode(PullDown);
@@ -116,7 +119,7 @@
int points[whichnum][3];
changeColor(&points[0][0]);
- if(read_keter())keter=true;
+ //if(read_keter())keter=true;
//bt.putc('a');
setMove(points[point_count][0],points[point_count][1]);
serial.printf("%s\n","mpu_setup");
@@ -126,18 +129,20 @@
serial.printf("%s\n","get_off");
//bt.putc('c');
get_offset();
+ serial.printf("%d\n",rez());
serial.printf("%s\n","move2");
//bt.putc('d');
}
Move2(0,0);
change_rm.start();
move_tm.start();
+ go_up_tm.start();
//bt.putc('e');
/*
*/
while(1) {
- serial.printf("%f",rorix);
+ serial.printf("%f",all);
serial.printf("%s",":");
if(!keter){
readGyz(&gz);
@@ -173,22 +178,13 @@
}
/////////ロリコン値取得///////
if(activeMove){
- RX.read(&rorix_l);
- RY.read(&roriy_l);
- rorix = rorix_l;
- rorix = rorix/Resolution;//回転数
- rorix *= Cir;
- roriy = roriy_l;
- roriy = roriy/Resolution;
- roriy *= Cir;
+ get_rorivol();
}
- //serial.printf("%f\n",roriy);
- ///////////////////////
///////符号反転処理////////
x_dis = rorix-off_rorix;
y_dis = roriy-off_roriy;
- //////////////////////
+
/*
bt.putc('g');
bt.puts(puc(x_dis));
@@ -200,7 +196,6 @@
/////移動処理/////////
if(!flag_finish_move){
float pwm = 0;
- //##########################problem############################x,y are cross このままだと半分になる
float distance;
if(points[point_count][0] == UP or points[point_count][0] == BACK){
distance = pointy-x_dis;
@@ -230,14 +225,7 @@
move_tm.reset();
while(move_tm.read_ms() < 600){
if(whileactive){
- RX.read(&rorix_l);
- RY.read(&roriy_l);
- rorix = rorix_l;
- rorix = rorix/Resolution;//回転数
- rorix *= Cir;
- roriy = roriy_l;
- roriy = roriy/Resolution;
- roriy *= Cir;
+ get_rorivol();
if(last_move_vertical){
set0_dis = roriy-off_roriy;
pwm = set0_dis*RoriGein;
@@ -290,54 +278,66 @@
//serial.printf("%f",pwm);
if(pwm > Max_pwm)pwm = Max_pwm;
if(activeMove){
-
- if(points[point_count][2] == 1){//しーつ
- if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
- array_count++;
- Move2(10,0);
-
- }
- }else if(points[point_count][2] == -1){//ばしゅたおる
+ if(points[point_count][2] != 0){
+ bool issheets = (points[point_count][2] == 1)?1:0;
+ if(issheets){
+ if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
+ if(array_count == 0)go_up = true;
+ array_count++;
+ point_move = true;
+ activeMove = false;
+ Move2(0,0);
+ //goto START;
+ }
+ }else{
if(points[point_count][1]-distance > nomal_towel[array_count] && array_count != 3){
if(array_count == 0)go_up = true;
array_count++;
point_move = true;
+ activeMove = false;
+ Move2(0,0);
+ //goto START;
+ }
+ }
+ if(point_move){
+ serial.printf("%s\n","maemae");
+ if(C_vari == 0){
+ off_rorik = rorix;
+ C_vari = 1;
+ }
+ if(go_up){
+ dis_go_up = abs(rorix-off_rorix);
+ if(abs(150-dis_go_up)*RoriGein > Min_pwm){
+ if(dis_go_up < 150){
+ Move2(0,(150-dis_go_up)*RoriGein);//+-20mm
+ go_up_tm.reset();
+ }else{
+ Move2(1,(dis_go_up-150)*RoriGein);
+ go_up_tm.reset();
}
}
+
+ if(go_up_tm.read_ms() > 300){
+ go_up = false;
+ Move2(0,0);
+ }
+ }else{
+ serial.printf("%s\n","owari");
+ outs = 1;
+ while(!ins.read())serial.printf("%d\n",ins.read());
+ outs = 0;
+ point_move = false;
+ }
+
+ }
+ }
+
if(!point_move){
if(distance > 0)Move2(points[point_count][0],pwm);
else{
pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein;
Move2(sewomukeru[points[point_count][0]],pwm);
}
- }else{
- //Move2(10,0);
- //wait(1);
- serial.printf("%s\n","maemae");
- if(C_vari == 0){
- off_rorik = rorix;
- C_vari = 1;
- }
- if(go_up){
- if(abs(rorix - off_rorik) > 150){
- go_up = false;
- Move2(0,0);
- //if(array_count != 0)point_move = false;
- }else{
- serial.printf("%f\n",rorik-off_rorik);//-4560
- Move2(0,0.1f);
- }
- }else{
- serial.printf("%s\n","owari");
- //Move2(10,0);
- //wait(1);
-
- outs = 1;
- while(!ins.read())serial.printf("%d\n",ins.read());
- outs = 0;
- point_move = false;
- }
-
}
serial.printf("%f",pwm);
serial.printf("%s",":");
@@ -376,6 +376,7 @@
}
+
void setMove(int p0,int p1){
trans(p0,p1,&pointx,&pointy,&sign);//一回だけ
array_count = 0;
@@ -390,19 +391,13 @@
else y_flag = false;
}
-
-void rolling(){
- while(pwm > Min_pwm){
- void_read_gyro();
- pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
- if(all < Min_ang)Move2(8,pwm);
- else if(all > -Min_ang)Move2(9,pwm);
- }
- }
-
-void void_read_gyro(){
- readGyz(&gz);
- offset_adjust(&all,&gz);
- to_signed(&all);
- }
-
\ No newline at end of file
+void get_rorivol(){
+ RX.read(&rorix_l);
+ RY.read(&roriy_l);
+ rorix = rorix_l;
+ rorix = rorix/Resolution;//回転数
+ rorix *= Cir;
+ roriy = roriy_l;
+ roriy = roriy/Resolution;
+ roriy *= Cir;
+}
\ No newline at end of file