Ryu Kaz
t6est
Diff: main.cpp
- Revision:
- 0:3dc012104243
- Child:
- 1:bbf776e6c792
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Sep 24 13:35:31 2019 +0000
@@ -0,0 +1,401 @@
+
+#include "mbed.h"
+#include "functions.h"
+#include "foot.h"
+#include "rori.h"
+#include "peripheral.h"
+
+#define UP 0
+#define BACK 1
+#define RIGHT 2
+#define LEFT 3
+#define RUP 4
+#define LUP 5
+#define RBACK 6
+#define LBACK 7
+#define RROLL 8
+#define LROLL 9
+
+//767ってちょっと765っぽいよね
+//lets go fiver time!
+int sewomukeru[10] = {
+ BACK,UP,LEFT,RIGHT,LBACK,RBACK,LUP,RUP,LROLL,RROLL//reverce array
+ };
+//もはやJava
+Serial serial(USBTX,USBRX);
+
+DigitalOut outs(PF_6);
+DigitalIn ins(PF_7);
+DigitalOut outt(D0);
+DigitalIn intt(D1);
+
+
+//https://github.com/ARMmbed/mbed-os/blob/master/targets/TARGET_STM/TARGET_STM32F7/TARGET_STM32F767xI/TARGET_NUCLEO_F767ZI/PeripheralPins.c
+
+
+double all = 0;
+bool keter = false;
+double gz;
+Timer change_rm;
+Timer move_tm;
+Timer go_up_tm;
+///////////定数たち//////////////
+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.05f
+const int R = 103;//これ直径やんけ
+const int Resolution = 1024;
+const float RoriGein = 0.001f;
+///////////////////////
+int point_count = 0;
+int array_count = 0;
+int C_vari = 0;
+/////////////////Move/////////////////////////
+/*
+int points[][3] = {
+ {RIGHT,3700,-1},
+ {LEFT,3700,0}
+ };
+*/
+int Robox = getRobox();
+//int C = (1750-Robox);
+int C = 1550;
+/*
+int sheets[] = {
+ C+100,C+1300,C+2500
+ };
+*/
+int sheets[] = {
+ 900,1700,3500
+ };
+int nomal_towel[]{
+ 1750,2775,3575//2130,2930,3730
+ };
+int final_towel[]{
+ C+410,C+1070,C+1730,C+2390
+ };
+float Cir = R * 3.14159f;
+long rorix_l,roriy_l;
+float pointx,pointy;
+float rorix = 0;
+float roriy = 0;
+float off_rorix,off_roriy;
+int sign;
+bool x_flag,y_flag;
+bool last_move_vertical,last_move_side;
+float x_dis,y_dis;
+bool flag_finish_move = false;
+
+void setMove(int p0,int p1);
+void get_rorivol();
+
+float pwm;
+bool activeMove = false;
+RORI RX(PF_9,PF_8);
+RORI RY(PA_4_ALT0,PB_0_ALT0);
+
+int abspointx = 0;
+int abspointy = 0;
+bool final = false;
+long rorik_l = 0;
+float rorik = 0;
+float rorik_temp = 0;
+float off_rorik = 0;
+
+bool point_move = false;
+
+bool go_up = false;
+float dis_go_up = 0;
+//////////////////////////////////////////////
+int main() {
+ ins.mode(PullDown);
+
+ setup_peri();
+ int whichnum = read_peri();
+ int points[whichnum][3];
+ changeColor(&points[0][0]);
+
+ //if(read_keter())keter=true;
+ //bt.putc('a');
+ setMove(points[point_count][0],points[point_count][1]);
+ serial.printf("%s\n","mpu_setup");
+ //bt.putc('b');
+ if(!keter){
+ mpu_setup();
+ 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",all);
+ serial.printf("%s",":");
+ if(!keter){
+ readGyz(&gz);
+ offset_adjust(&all,&gz);
+ //bt.putc('f');
+ //bt.puts(puc(all));
+ //bt.putc('F');
+ to_signed(&all);
+ }
+ /////////////傾き修正////////////////////////////
+
+ if(!keter){
+ pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
+ if(!activeMove){
+ if(pwm <= Min_pwm){
+ //bt.putc('k');
+ activeMove = true;
+ }
+
+ if(all < Min_ang)Move2(8,pwm);
+ else if(all > -Min_ang)Move2(9,pwm);
+ //else Move2(9,0);
+ /////////////////////////////////////
+
+ serial.printf("%f",pwm);
+ serial.printf("%s",":");
+ serial.printf("%f",all);
+ serial.printf("%s\n",":");
+
+ }
+ }else{
+ if(!activeMove)activeMove = true;
+ }
+ /////////ロリコン値取得///////
+ if(activeMove){
+ get_rorivol();
+ }
+
+ ///////符号反転処理////////
+ x_dis = rorix-off_rorix;
+ y_dis = roriy-off_roriy;
+
+ /*
+ bt.putc('g');
+ bt.puts(puc(x_dis));
+ bt.putc('G');
+ bt.putc('h');
+ bt.puts(puc(y_dis));
+ bt.putc('H');
+ */
+ /////移動処理/////////
+ if(!flag_finish_move){
+ float pwm = 0;
+ float distance;
+ if(points[point_count][0] == UP or points[point_count][0] == BACK){
+ distance = pointy-x_dis;
+ last_move_vertical = true;
+ last_move_side = false;
+ }else if(points[point_count][0] == LEFT or points[point_count][0] == RIGHT){
+ distance = pointx-y_dis;
+ last_move_vertical = false;
+ last_move_side = true;
+ }
+ //else if(points[point_count][0] >= 4 && points[point_count][0] <= 7)ななっめ移動
+ if(x_flag or y_flag)distance *= -1;
+ pwm = distance*RoriGein;
+ pwm = (pwm < 0)?-pwm:pwm;//abs
+ if(pwm < Min_pwm){
+ //次の処理へ
+ serial.printf("%d\n",move_tm.read_ms());
+ if(move_tm.read_ms() >= 600){
+ serial.printf("%s","waiting");
+ serial.printf("%d\n",point_count);
+ //bt.putc('i');
+ //////////////////////////////
+ pwm = 1;
+ float set0_dis;
+ float pwm_while;
+ bool whileactive = true;
+ move_tm.reset();
+ while(move_tm.read_ms() < 600){
+ if(whileactive){
+ get_rorivol();
+ if(last_move_vertical){
+ set0_dis = roriy-off_roriy;
+ pwm = set0_dis*RoriGein;
+ if(abs(pwm) >= Min_pwm)move_tm.reset();
+ if(pwm < 0){
+ Move2(RIGHT,-pwm);
+ }else{
+ Move2(LEFT,pwm);
+ }
+ }else if(last_move_side){
+ serial.printf("%s\n","side");
+ set0_dis = rorix-off_rorix;
+ serial.printf("%f\n",set0_dis);
+ pwm = set0_dis*RoriGein;
+ if(abs(pwm) >= Min_pwm)move_tm.reset();
+ if(pwm < 0){
+ Move2(UP,-pwm);
+ }else{
+ Move2(BACK,pwm);
+ }
+ }
+ }else{
+ //serial.printf("%s\n","whileroll");
+ pwm_while = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
+ if(pwm_while <= Min_pwm){
+ whileactive = true;
+ }
+ if(all < Min_ang)Move2(8,pwm_while);
+ else if(all > -Min_ang)Move2(9,pwm_while);
+ else Move2(9,0);
+ }
+ if(whileactive){
+ if(change_rm.read_ms() > 500){
+ whileactive = false;
+ change_rm.reset();
+ serial.printf("%s\n","change_rm_while");
+ }
+ }
+ }
+ ////////////////////////
+ point_count++;
+ Move2(1,0);
+ if(point_count >= sizeof(points)/sizeof(*points)){
+ flag_finish_move = true;
+ }else{
+ setMove(points[point_count][0],points[point_count][1]);
+ }
+ }
+ }else{
+ //serial.printf("%f",pwm);
+ if(pwm > Max_pwm)pwm = Max_pwm;
+ if(activeMove){
+ 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);
+ }
+ }
+ serial.printf("%f",pwm);
+ serial.printf("%s",":");
+ serial.printf("%f",distance);
+ serial.printf("%s",":");
+ serial.printf("%d",point_count);
+ serial.printf("%s",":");
+ serial.printf("%d",sizeof(points)/sizeof(*points));
+ serial.printf("%s",":");
+ serial.printf("%d",y_flag);
+ serial.printf("%s",":");
+ if(points[1][0] == RIGHT)serial.printf("%s","orange");
+ if(points[1][0] == LEFT)serial.printf("%s","blue");
+ serial.printf("%s",":");
+ if(points[0][1] > 5100)serial.printf("%s\n","sheets");
+ else serial.printf("%s\n","towel");
+ }
+ move_tm.reset();
+ }
+
+ }else{
+ serial.printf("%s","finish");
+ }
+
+ if(activeMove){
+ if(change_rm.read_ms() > 1500){
+ activeMove = false;
+ change_rm.reset();
+ //bt.putc('j');
+ serial.printf("%s\n","change_rm");
+ }
+ }
+
+
+ }
+
+}
+
+
+void setMove(int p0,int p1){
+ trans(p0,p1,&pointx,&pointy,&sign);//一回だけ
+ array_count = 0;
+ off_rorix = rorix;
+ off_roriy = roriy;
+ abspointx = (pointx < 0)?-pointx:pointx;
+ abspointy = (pointy < 0)?-pointy:pointy;
+ //一回
+ if((sign & 0b01) == 1)x_flag = true;//x < 0
+ else x_flag = false;
+ if((sign >> 1) == 1)y_flag = true;//y < 0
+ else y_flag = false;
+ }
+
+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