Ryu Anzawa
to takasou
Dependencies: SerialMultiByte QEI chatteringremoval omni_wheel prototype01 towelest PID ikarashiMDC_2byte_ver air2 OmniPosition PS3
Diff: main.cpp
- Revision:
- 2:d01bc3dcb247
- Parent:
- 1:e7413a7b013b
--- a/main.cpp Mon Sep 23 23:30:38 2019 +0000
+++ b/main.cpp Fri Sep 27 10:50:13 2019 +0000
@@ -1,362 +1,433 @@
-#include "mbed.h" //Red court
-#include "ikarashiMDC.h"
+//#include "mbed.h" //Red court
+//#include "ikarashiMDC.h"
+//#include "omni_wheel.h"
+//#include "proto01.h"
+//#include "OmniPosition.h"
+//#include "SerialMultiByte.h"
+//#include "PID.h"
+//#include "PS3.h"
+//#include "air.h"
+//#include "towelest.h"
+//#include "chatteringremoval.h"
+//#include "pinconfig_main.h"
+//
+//PID angle(10.0,0.2,0.000095,0.01);
+//OmniWheel omni(4);
+//OmniPosition posi(main_0TX,main_0RX);
+//Serial serial(mdTX, mdRX, 115200);
+//RawSerial pc(USBTX, USBRX, 115200);
+//SerialMultiByte sensor(main_1TX,main_1RX);
+//towelest towel(&serial,2,schmitt_trigger_0,schmitt_trigger_1,schmitt_trigger_2);
+//air air;
+//PS3 ps3(FEPTX,FEPRX);
+//chatteringremoval button1(1.0);
+//DigitalOut sw1(hijoteisi);
+//DigitalOut LED(LED1);
+//Timer timer;
+//
+//int num = 0, X, Y, b[12], stick[4], trigger[2], loli[3], b+[12];
+//int X0,Y0,X1,Y1;
+//double valueX, valueY, value[4],mecha_power[4],spin_power=0, IRdistance[2],TFdistance[2],x2, y2;
+//
+//union UnionBytes{
+// uint8_t bytes[28];
+// float IRdist[7]; //0~1
+// int32_t TFdist[7]; //2~3
+// int32_t pulses[7]; //4~6
+//};
+//
+//ikarashiMDC motor[]=
+//{
+// ikarashiMDC(0,0,SM,&serial),
+// ikarashiMDC(0,1,SM,&serial),
+// ikarashiMDC(0,2,SM,&serial),
+// ikarashiMDC(0,3,SM,&serial),
+// ikarashiMDC(1,0,SM,&serial),
+// ikarashiMDC(1,1,SM,&serial),
+// ikarashiMDC(1,2,SM,&serial),
+// ikarashiMDC(1,3,SM,&serial)
+//};
+//
+//Proto1 proto(500, 700, 0.4, 0.14);
+//
+//
+//void getSensorData() //Receive sensor value
+//{
+// UnionBytes bytedata;
+// sensor.getData(bytedata.bytes);
+// for(int i=0; i<2; i++){
+// IRdistance[i] = bytedata.IRdist[i];
+// }
+// for(int i=0; i<2; i++){
+// TFdistance[i] = bytedata.TFdist[i+2];
+// }
+// for(int i=0; i<3; i++){
+// loli[i] = bytedata.pulses[i+4];
+// }
+//}
+//
+//void clossmove0() //Adjustment,Airfront
+//{
+// if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
+// x2=0;
+// y2=0.3;
+// }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
+// x2=0;
+// y2=-0.3;
+// }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
+// x2=-0.3;
+// y2=0;
+// }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
+// x2=0.3;
+// y2=0;
+// }else{
+// x2=0;
+// y2=0;
+// }
+//}
+//
+//void clossmove1() //Adjustment,Recoveryfront
+//{
+// if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
+// x2=0.3;
+// y2=0;
+// }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
+// x2=-0.3;
+// y2=0;
+// }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
+// x2=0;
+// y2=0.3;
+// }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
+// x2=0;
+// y2=-0.3;
+// }else{
+// x2=0;
+// y2=0;
+// }
+//}
+//
+//void clossmove2() //Adjustment,Dropfront
+//{
+// if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
+// x2=0;
+// y2=-0.3;
+// }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
+// x2=0;
+// y2=0.3;
+// }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
+// x2=0.3;
+// y2=0;
+// }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
+// x2=-0.3;
+// y2=0;
+// }else{
+// x2=0;
+// y2=0;
+// }
+//}
+//
+//void clossmove3() //Adjustment,Towelfront
+//{
+// if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
+// x2=-0.3;
+// y2=0;
+// }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
+// x2=0.3;
+// y2=0;
+// }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
+// x2=0;
+// y2=-0.3;
+// }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
+// x2=0;
+// y2=0.3;
+// }else{
+// x2=0;
+// y2=0;
+// }
+//}
+//
+//void Recovery() //Recovery mechanism
+//{
+// /* 洗濯物を挟む機構 */
+// if((X0 > 170)&&(Y0 > 50 && Y0 < 200)) {
+// mecha_power[1] = 0.6;
+// }else if((X0 < 100)&&(Y0 > 50 && Y0 < 200)) {
+// mecha_power[1] = -0.6;
+// }else{
+// mecha_power[1] = 0;
+// }
+// if((X1 > 170)&&(Y1 > 50 && Y1 < 200)) {
+// mecha_power[2] = 0.6;
+// }else if((X1 < 100)&&(Y1 > 50 && Y1 < 200)) {
+// mecha_power[2] = -0.6;
+// }else{
+// mecha_power[2] = 0;
+// }
+// /* 洗濯物を挟む機構の首 */
+// if (b[10] == 1 && b[11] == 0) {
+// mecha_power[0] = 0.3;
+// } else if (b[10] == 0 && b[11] == 1) {
+// mecha_power[0] = -0.3;
+// } else {
+// mecha_power[0] =0.0;
+// }
+// mecha_power[3] = 0;
+//
+// motor[4].setSpeed(mecha_power[0]);
+// motor[5].setSpeed(mecha_power[1]);
+// motor[6].setSpeed(mecha_power[2]);
+// motor[7].setSpeed(mecha_power[3]);
+//}
+//
+//void stop()
+//{
+// for(int i=4;i<8;i++) motor[i].setSpeed(0);
+//}
+//
+//void Drop()
+//{
+// if(b[4] == 1 && b[5] == 0){
+// mecha_power[3] = 0.5;
+// }else if(b[4] == 0 && b[5] == 1) {
+// mecha_power[3] = -0.5;
+// }else{
+// mecha_power[3] = 0;
+// }
+// motor[7].setSpeed(mecha_power[3]);
+//}
+//
+//int main()
+//{
+// sw1 = 1;
+// double nowX,nowY,t;
+//
+// for(int i=0;i<4;i++)motor[i].braking = false;
+// omni.wheel[0].setRadian(PI * 1 / 4);
+// omni.wheel[1].setRadian(PI * 3 / 4);
+// omni.wheel[2].setRadian(PI * 5 / 4);
+// omni.wheel[3].setRadian(PI * 7 / 4);
+// angle.setInputLimits(-6.28,6.28);
+// angle.setOutputLimits(-0.4,0.4);
+// angle.setBias(0);
+// angle.setMode(1);
+// angle.setSetPoint(0);
+//
+// button1.countreset();
+//
+// sensor.startReceive(28);
+//
+// while(1){
+// LED = !LED;
+//
+// towel.setPulse(loli[2]);
+//
+//// PS3 value
+// for(int i = 0; i < 12; i++) {
+// b[i] = ps3.getButton(i);
+// pc.printf("%2d", b[i] );
+// }
+// for(int i = 0; i < 4; i++) {
+// stick[i] = ps3.getStick(i);
+// pc.printf("%4d", stick[i] );
+// }
+// X0 = stick[0];
+// Y0 = stick[1];
+// X1 = stick[2];
+// Y1 = stick[3];
+//
+// for(int i = 0; i < 2; i++) {
+// trigger[i] = ps3.getTrigger(i);
+//// pc.printf("%4d",trigger[i]);
+// }
+//
+// //Sensor value
+// getSensorData();
+//
+// //Count
+// button1.assignvalue(b[9]);
+// num = button1.getCount();
+//
+// /*【Action】*/
+// switch(num){
+// case 0: angle.setSetPoint(0);
+// clossmove0();
+// Recovery();
+// towel.open();
+// air.solenoid1_open();
+// nowX = posi.getX();
+// nowY = posi.getY();
+// break;
+// case 1: proto.targetXY(-3000, 0, nowX ,nowY);
+// towel.allstop();
+// stop();
+// break;
+// case 2: angle.setSetPoint(1.57);
+// Drop();
+// clossmove3();
+// towel.allstop();
+// stop();
+// nowX = posi.getX();
+// nowY = posi.getY();
+// break;
+// case 3: proto.targetXY(-2000, 3000, nowX ,nowY);
+// towel.allstop();
+// stop();
+// if((posi.getX()>-2200 && posi.getX()<-1800) && (posi.getY()>2800 && posi.getY()<3200)){
+// num = 4;
+// angle.setSetPoint(0);
+// }
+// break;
+// case 4: clossmove0();
+//// Recovery();
+//// nowX = posi.getX();
+//// nowY = posi.getY();
+//// break;
+//// case 5: proto.targetXY(-3000, 0, nowX ,nowY);
+//// break;
+//// case 6: angle.setSetPoint(1.57);
+//// Drop();
+//// clossmove3();
+//// nowX = posi.getX();
+//// nowY = posi.getY();
+//// break;
+//// case 7: proto.targetXY(-3000,2600,-3000,0);
+//// if(posi.getX()>-3200 && posi.getX()<-2800 && posi.getY()>2400 && posi.getY()<2800){
+//// num = 8;
+//// }
+//// break;
+//// case 8: angle.setSetPoint(1.57);
+//// clossmove3();
+//// nowX = posi.getX();
+//// nowY = posi.getY();
+////
+//// towel.drop();
+//// t = timer.read();
+//// if(b[4] == 1 && b[5] == 1){
+//// timer.start();
+//// }
+//// if(t > 5){
+//// towel.close();
+//// }else if(t > 20){
+//// angle.setSetPoint(0);
+//// air.solenoid2_open();
+//// }
+//// break;
+//// case 9: angle.setSetPoint(0);
+//// proto.targetXY(0, 0,nowX ,nowY);
+//// break;
+// default: motor[0].setSpeed(0);
+// motor[1].setSpeed(0);
+// motor[2].setSpeed(0);
+// motor[3].setSpeed(0);
+//
+// air.solenoid1_close();
+// air.solenoid2_close();
+//
+// }
+//
+// angle.setProcessValue(posi.getTheta());
+// spin_power = -angle.compute();
+//
+// if(num == 0 || num == 2 || num == 4 || num == 6 || num == 8){
+// omni.computeXY(x2,y2,spin_power);
+// }else{
+// proto.Input_nowXY(posi.getX(),posi.getY());
+// proto.calculate();
+// omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
+// }
+// /*
+// omni.conputeCircular(proto.getValue(), proto.gettheta(), spin_power);
+// */
+//
+// for(int i = 0; i < 4; i++){
+// value[i] = omni.wheel[i];
+// motor[i].setSpeed(value[i]);
+//// pc.printf("%f ", value[i]);
+// }
+//
+//// wait(0.001);
+// pc.printf(" loli:%d case:%d",loli[2],num);
+//
+// //Wheel value
+//// pc.printf(" vector:%f pid:%.2f",proto.vector,spin_power);
+// //Odmetry
+//// pc.printf("x:%d y:%d θ:%f now:%f",posi.getX(),posi.getY(),posi.getTheta(),proto.nowDis);
+//
+// pc.printf("\r\n");
+// for(int i = 0; i < 12; i++) {
+// b+[i] = ps3.getButton(i);
+// }
+// }
+//}
+#include "mbed.h" //たかそうのロボット
+#include "ikarashiMDC.h"
#include "omni_wheel.h"
#include "proto01.h"
#include "OmniPosition.h"
-#include "SerialMultiByte.h"
#include "PID.h"
-#include "PS3.h"
-#include "air.h"
-#include "towelest.h"
-#include "chatteringremoval.h"
#include "pinconfig_main.h"
-PID angle(10.0,0.2,0.000095,0.01);
+PID angle(20.0,5.0,0.0000001,0.01);
OmniWheel omni(4);
-OmniPosition posi(main_0TX,main_0RX);
+OmniPosition posi(main_0TX, main_0RX);
Serial serial(mdTX, mdRX, 115200);
+DigitalOut serialcontrol(D10);
RawSerial pc(USBTX, USBRX, 115200);
-SerialMultiByte sensor(main_1TX,main_1RX);
-towelest towel(&serial,2,schmitt_trigger_0,schmitt_trigger_1,schmitt_trigger_2);
-air air;
-PS3 ps3(FEPTX,FEPRX);
-chatteringremoval button1(1.0);
-DigitalOut sw1(hijoteisi);
-DigitalOut LED(LED1);
-Timer timer;
-
-int num = 0, X, Y, b[12], stick[4], trigger[2], loli[3], b+[12];
-int X0,Y0,X1,Y1;
-double valueX, valueY, value[4],mecha_power[4],spin_power=0, IRdistance[2],TFdistance[2],x2, y2;
-
-union UnionBytes{
- uint8_t bytes[28];
- float IRdist[7]; //0~1
- int32_t TFdist[7]; //2~3
- int32_t pulses[7]; //4~6
-};
+DigitalOut sw1(PB_1);
+DigitalOut sw2();
ikarashiMDC motor[]=
{
ikarashiMDC(0,0,SM,&serial),
ikarashiMDC(0,1,SM,&serial),
ikarashiMDC(0,2,SM,&serial),
- ikarashiMDC(0,3,SM,&serial),
- ikarashiMDC(1,0,SM,&serial),
- ikarashiMDC(1,1,SM,&serial),
- ikarashiMDC(1,2,SM,&serial),
- ikarashiMDC(1,3,SM,&serial)
+ ikarashiMDC(0,3,SM,&serial)
};
-Proto1 proto(500, 700, 0.4, 0.14);
-
-
-void getSensorData() //Receive sensor value
-{
- UnionBytes bytedata;
- sensor.getData(bytedata.bytes);
- for(int i=0; i<2; i++){
- IRdistance[i] = bytedata.IRdist[i];
- }
- for(int i=0; i<2; i++){
- TFdistance[i] = bytedata.TFdist[i+2];
- }
- for(int i=0; i<3; i++){
- loli[i] = bytedata.pulses[i+4];
- }
-}
-
-void clossmove0() //Adjustment,Airfront
-{
- if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
- x2=0;
- y2=0.3;
- }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
- x2=0;
- y2=-0.3;
- }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
- x2=-0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
- x2=0.3;
- y2=0;
- }else{
- x2=0;
- y2=0;
- }
-}
-
-void clossmove1() //Adjustment,Recoveryfront
-{
- if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
- x2=0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
- x2=-0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
- x2=0;
- y2=0.3;
- }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
- x2=0;
- y2=-0.3;
- }else{
- x2=0;
- y2=0;
- }
-}
-
-void clossmove2() //Adjustment,Dropfront
-{
- if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
- x2=0;
- y2=-0.3;
- }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
- x2=0;
- y2=0.3;
- }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
- x2=0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
- x2=-0.3;
- y2=0;
- }else{
- x2=0;
- y2=0;
- }
-}
-
-void clossmove3() //Adjustment,Towelfront
-{
- if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
- x2=-0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
- x2=0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
- x2=0;
- y2=-0.3;
- }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
- x2=0;
- y2=0.3;
- }else{
- x2=0;
- y2=0;
- }
-}
-
-void Recovery() //Recovery mechanism
-{
- /* 洗濯物を挟む機構 */
- if((X0 > 170)&&(Y0 > 50 && Y0 < 200)) {
- mecha_power[1] = 0.6;
- }else if((X0 < 100)&&(Y0 > 50 && Y0 < 200)) {
- mecha_power[1] = -0.6;
- }else{
- mecha_power[1] = 0;
- }
- if((X1 > 170)&&(Y1 > 50 && Y1 < 200)) {
- mecha_power[2] = 0.6;
- }else if((X1 < 100)&&(Y1 > 50 && Y1 < 200)) {
- mecha_power[2] = -0.6;
- }else{
- mecha_power[2] = 0;
- }
- /* 洗濯物を挟む機構の首 */
- if (b[10] == 1 && b[11] == 0) {
- mecha_power[0] = 0.3;
- } else if (b[10] == 0 && b[11] == 1) {
- mecha_power[0] = -0.3;
- } else {
- mecha_power[0] =0.0;
- }
- mecha_power[3] = 0;
-
- motor[4].setSpeed(mecha_power[0]);
- motor[5].setSpeed(mecha_power[1]);
- motor[6].setSpeed(mecha_power[2]);
- motor[7].setSpeed(mecha_power[3]);
-}
-
-void stop()
-{
- for(int i=4;i<8;i++) motor[i].setSpeed(0);
-}
-
-void Drop()
-{
- if(b[4] == 1 && b[5] == 0){
- mecha_power[3] = 0.5;
- }else if(b[4] == 0 && b[5] == 1) {
- mecha_power[3] = -0.5;
- }else{
- mecha_power[3] = 0;
- }
- motor[7].setSpeed(mecha_power[3]);
-}
+Proto1 proto(500, 500, 0.4, 0.14);
int main()
{
sw1 = 1;
- double nowX,nowY,t;
-
- for(int i=0;i<4;i++)motor[i].braking = false;
+ int cnt=0;
+ motor[0].braking = true;
+ double valueX, valueY, value[4];
omni.wheel[0].setRadian(PI * 1 / 4);
omni.wheel[1].setRadian(PI * 3 / 4);
omni.wheel[2].setRadian(PI * 5 / 4);
omni.wheel[3].setRadian(PI * 7 / 4);
- angle.setInputLimits(-6.28,6.28);
+ angle.setInputLimits(-3.14,3.14);
angle.setOutputLimits(-0.4,0.4);
angle.setBias(0);
angle.setMode(1);
angle.setSetPoint(0);
-
- button1.countreset();
-
- sensor.startReceive(28);
-
while(1){
- LED = !LED;
-
- towel.setPulse(loli[2]);
-
-// PS3 value
- for(int i = 0; i < 12; i++) {
- b[i] = ps3.getButton(i);
- pc.printf("%2d", b[i] );
- }
- for(int i = 0; i < 4; i++) {
- stick[i] = ps3.getStick(i);
- pc.printf("%4d", stick[i] );
- }
- X0 = stick[0];
- Y0 = stick[1];
- X1 = stick[2];
- Y1 = stick[3];
-
- for(int i = 0; i < 2; i++) {
- trigger[i] = ps3.getTrigger(i);
-// pc.printf("%4d",trigger[i]);
- }
-
- //Sensor value
- getSensorData();
-
- //Count
- button1.assignvalue(b[9]);
- num = button1.getCount();
-
- /*【Action】*/
- switch(num){
- case 0: angle.setSetPoint(0);
- clossmove0();
- Recovery();
- towel.open();
- air.solenoid1_open();
- nowX = posi.getX();
- nowY = posi.getY();
- break;
- case 1: proto.targetXY(-3000, 0, nowX ,nowY);
- towel.allstop();
- stop();
- break;
- case 2: angle.setSetPoint(1.57);
- Drop();
- clossmove3();
- towel.allstop();
- stop();
- nowX = posi.getX();
- nowY = posi.getY();
- break;
- case 3: proto.targetXY(-2000, 3000, nowX ,nowY);
- towel.allstop();
- stop();
- if((posi.getX()>-2200 && posi.getX()<-1800) && (posi.getY()>2800 && posi.getY()<3200)){
- num = 4;
- angle.setSetPoint(0);
- }
- break;
- case 4: clossmove0();
-// Recovery();
-// nowX = posi.getX();
-// nowY = posi.getY();
-// break;
-// case 5: proto.targetXY(-3000, 0, nowX ,nowY);
-// break;
-// case 6: angle.setSetPoint(1.57);
-// Drop();
-// clossmove3();
-// nowX = posi.getX();
-// nowY = posi.getY();
-// break;
-// case 7: proto.targetXY(-3000,2600,-3000,0);
-// if(posi.getX()>-3200 && posi.getX()<-2800 && posi.getY()>2400 && posi.getY()<2800){
-// num = 8;
-// }
-// break;
-// case 8: angle.setSetPoint(1.57);
-// clossmove3();
-// nowX = posi.getX();
-// nowY = posi.getY();
-//
-// towel.drop();
-// t = timer.read();
-// if(b[4] == 1 && b[5] == 1){
-// timer.start();
-// }
-// if(t > 5){
-// towel.close();
-// }else if(t > 20){
-// angle.setSetPoint(0);
-// air.solenoid2_open();
-// }
-// break;
-// case 9: angle.setSetPoint(0);
-// proto.targetXY(0, 0,nowX ,nowY);
-// break;
- default: motor[0].setSpeed(0);
- motor[1].setSpeed(0);
- motor[2].setSpeed(0);
- motor[3].setSpeed(0);
-
- air.solenoid1_close();
- air.solenoid2_close();
-
- }
-
angle.setProcessValue(posi.getTheta());
- spin_power = -angle.compute();
-
- if(num == 0 || num == 2 || num == 4 || num == 6 || num == 8){
- omni.computeXY(x2,y2,spin_power);
- }else{
- proto.Input_nowXY(posi.getX(),posi.getY());
- proto.calculate();
- omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
- }
- /*
- omni.conputeCircular(proto.getValue(), proto.gettheta(), spin_power);
- 機体の向きに関係ないスティック操作をしたい場合はspin_powerに、スティックの角度（atan2(スティックのy座標,スティックのx座標)）を足せば良い
- */
+ double spin_power = -angle.compute();
+ /*ここから*/
+ if (cnt==700) proto.targetXY(2000, 3000, posi.getX(), -posi.getY());
+ if (cnt==1400) proto.targetXY(2000, 6000, posi.getX(), -posi.getY());
+ if (cnt==2100) proto.targetXY(0, 6000, posi.getX(), -posi.getY());
+ if (cnt==2800) proto.targetXY(0, 3000, posi.getX(), -posi.getY());
+ if (cnt==3500) proto.targetXY(0, 0, posi.getX(), -posi.getY());
+// if (cnt==3000) proto.targetXY(0, 0, posi.getX(), posi.getY());
+ proto.Input_nowXY(posi.getX(), -posi.getY());
+ proto.calculate();
+ omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
+ /*ここまで*/
for(int i = 0; i < 4; i++){
value[i] = omni.wheel[i];
- motor[i].setSpeed(value[i]);
-// pc.printf("%f ", value[i]);
+// pc.printf("%.2f || ",value[i]);
}
-
-// wait(0.001);
- pc.printf(" loli:%d case:%d",loli[2],num);
+ pc.printf("x:%d || y:%d || θ:%f || vector:%f || now:%f || c:%f"
+ , posi.getX(), -posi.getY(), posi.getTheta(), proto.vector, proto.nowDis, proto.counter);
- //Wheel value
-// pc.printf(" vector:%f pid:%.2f",proto.vector,spin_power);
- //Odmetry
-// pc.printf("x:%d y:%d θ:%f now:%f",posi.getX(),posi.getY(),posi.getTheta(),proto.nowDis);
-
- pc.printf("\r\n");
- for(int i = 0; i < 12; i++) {
- b+[i] = ps3.getButton(i);
+ for(int i = 0; i < 4; i++) {
+ if (fabs(value[i]) < 0.05) value[i] = 0;
+ motor[i].setSpeed(value[i]);
}
+ cnt++;
+ pc.printf("||pid:%.2f\r\n",spin_power);
}
-}
+}
\ No newline at end of file