void transfer_To_MiddleLand(int repeat_Num, Target target)
{
ColorBoundary color_B = classifyObject(target.flag);
printf("\n--------------------see down------------------\n\n");
int i;
// 물체 차고 재접근의 루프
for(i=0; i<repeat_Num; i++){
set_Position(NOTHING, PLASTIC);
// 이제 물체 차야함
left_Kick();
usleep(200000);
// 물체앞까지 의무걸음
moveStraight();
usleep(5500000); // 5걸음
moveStop();
usleep(200000);
}
}
void Player::updateDecision() {
float temp = 0;
b2Vec2 tempDir = b2Vec2(target.x-body->GetWorldCenter().x,target.y-body->GetWorldCenter().y);
float up = (tempDir.x*body->GetLinearVelocity().x + tempDir.y*body->GetLinearVelocity().y);
float down = sqrt(tempDir.x*tempDir.x+tempDir.y*tempDir.y)*sqrt(body->GetLinearVelocity().x*body->GetLinearVelocity().x+
body->GetLinearVelocity().y*body->GetLinearVelocity().y);
b2Vec2 velocity = body->GetLinearVelocity();
b2Vec2 newVelocity;
newVelocity.x = velocity.x*cos(1)+velocity.y*sin(1);
newVelocity.y = -velocity.x*sin(1)+velocity.y*cos(1);
float angle = acos(up/down);
temp = acos((tempDir.x*velocity.x + tempDir.y*velocity.y)/
(sqrt(tempDir.x*tempDir.x+tempDir.y*tempDir.y)*sqrt(velocity.x*velocity.x+velocity.y*velocity.y)));
if(static_cast<int>(angle) != 0)
rotateRight();
else
rotateStop();
if(static_cast<int>(body->GetWorldCenter().x-target.x) == 0 &&
static_cast<int>(body->GetWorldCenter().y-target.y) == 0)
linearImpulse = 0;
else
moveUp();
if(static_cast<unsigned>(body->GetAngle()) > 4*M_PI) {
moveStop();
if(!curRotState)
moveUp();
}
}
void go_MiddlePos()
{
moveStraightGrabbing();
usleep(20000000);
moveStop();
}
int approach2Milk(VideoCopy *image, Target obj) {
ColorBoundary color_B = classifyObject(obj.flag);
int isFind;
prevTarget.x_point = prevTarget.y_point = -1;
while(1)
{
// search
isFind = toleranceSearch(image, &obj, color_B);
//printf("isFind : %d\n", isFind);
printf("y : %d\n", obj.y_point);
if(isFind <= 0) // stop_Signal
break;
// if(obj.y_point < 20)
// break;
// command -> robot
else{
prevTarget = obj;
// printf("pass command\nx_mid : %d\n", obj.x_point);
int wPix = (MAX_X/2 - obj.x_point);
// printf("wPix : %d\n", wPix);
int direction = wPix < 0 ? 0 : 1;
wPix = abs(wPix);
if(wPix > 60){
if(direction == 0)
moveLeft();
else if(direction == 1)
moveRight();
}
else{
moveStraight();
}
}
}
moveStop();
printf("approach complete. isFind : %d\n", isFind);
if (prevTarget.x_point == -1) { // 아예 처음부터 못 찾은 경우
return 2;
} else if (prevTarget.y_point > 80) { // 갑자기 사라졌다고 의심할 수 있을 경우
return 1;
} else {
return 0;
}
}
void go_MiddlePos(int sec)
{
moveStraightGrabbing();
sec = sec* 1000000;
usleep(sec);
moveStop();
}
//
// void updateState()
// Last modified: 27Aug2006
//
// Updates the state of the cell based upon the
// current states of the neighbors of the cell.
//
// Returns: <none>
// Parameters: <none>
//
void Cell::updateState()
{
Neighbor currNbr;
for (GLint i = 0; i < getNNbrs(); ++i)
{
if (!getHead(currNbr)) break;
// change formation if a neighbor has changed formation
if (getNbr(0)->formation.getFormationID() > formation.getFormationID())
changeFormation(getNbr(0)->formation, *getNbr(0));
getNbr(0)->relActual = getRelationship(currNbr.ID);
++(*this);
}
rels = getRelationships();
if(rels.getSize())
{
// reference the neighbor with the smallest gradient
// to establish correct position in formation
Neighbor *refNbr = nbrWithMinGradient();
Relationship *nbrRel = relWithID(refNbr->rels, ID);
if ((formation.getSeedID() != ID) && (refNbr != NULL) && (nbrRel != NULL))
{
// error (state) is based upon the accumulated error in the formation
nbrRel->relDesired.rotateRelative(-refNbr->rotError);
GLfloat theta = scaleDegrees(nbrRel->relActual.angle() -
(-refNbr->relActual).angle());
rotError = scaleDegrees(theta + refNbr->rotError);
transError = nbrRel->relDesired - nbrRel->relActual +
refNbr->transError;
transError.rotateRelative(-theta);
if (transError.norm() > threshold()) moveArc(transError);
else
if (abs(rotError) > angThreshold())
moveArc(0.0, degreesToRadians(-rotError));
/*if (abs(scaleDegrees(refNbr->relActual.angle() -
refNbr->relDesired.angle())) > angThreshold())
orientTo(refNbr->relActual, refNbr->relDesired.angle());*/
else moveStop();
}
else moveStop();
}
} // updateState()
int approach2Milk(VideoCopy *image, Target obj) {
ColorBoundary color_B = classifyObject(obj.flag);
int isFind;
int cnt = 0;
while(1)
{
// search
isFind = toleranceSearch(image, &obj, color_B);
//printf("isFind : %d\n", isFind);
printf("y : %d\n", obj.y_point);
if(isFind <= 0) // stop_Signal
{
cnt++;
if(cnt > 0)
break;
}
// if(obj.y_point < 20)
// break;
// command -> robot
else{
// printf("pass command\nx_mid : %d\n", obj.x_point);
int wPix = (MAX_X/2 - obj.x_point);
// printf("wPix : %d\n", wPix);
int direction = wPix < 0 ? 0 : 1;
wPix = abs(wPix);
if(wPix > 60){
if(direction == 0)
moveLeft();
else if(direction == 1)
moveRight();
}
else{
moveStraight();
}
}
}
moveStop();
printf("approach complete. isFind : %d\n", isFind);
usleep(200000);
}
/*!
* Key for UP was pressed
*/
void BlindSwitchIn::OnPressUp(){
switch (_State) {
case BlindMovingDown:
_State = BlindSomeWhere;
moveStop();
break;
default:
_State = BlindMovingUp;
moveUp();
}
}
void auto_mode()
{
while(1) {
//printf("Auto Mode: Enter 's' to stop and 'm' for manual mode\r\n");
char mode = getchar();
//if( bit_is_set(UCSR0A, RXC0) != 0) {
// char mode = UDR0;
if(mode == 's') {
moveStop();
printf("Robot Stops\r\n");
}
if(mode == 'm') {
printf("Manual mode enabled\r\n");
manual_mode();
}
// }else{
// _delay_ms(50);
// int adc_val0=ReadADC(0);
// _delay_ms(50);
// int adc_val1=ReadADC(1);
// _delay_ms(50);
// int adc_val2=ReadADC(2);
// printf("Auto Mode - Sensor Readings : ClifL = %d, ClifR = %d, Bump = %d\r\n",adc_val0,adc_val1,adc_val2);
//
// if( (adc_val0 < 100) ) {
// //if( (adc_val0 < 100) | (adc_val1 < 100) | (adc_val2 < 100) ) {
// moveStop();
// printf("Robot stops\r\n");
// _delay_ms(0500);
//
// moveBackward(160,160);
// printf("Robot moves backward\r\n");
// _delay_ms(1000);
//
// moveStop();
// printf("Robot stops\r\n");
// _delay_ms(0500);
//
// moveRight(160,160);
// printf("Robot moves Right\r\n");
// _delay_ms(0500);
//
// moveStop();
// printf("Robot stops\r\n");
// _delay_ms(0500);
// }else{
// moveForward(160,160);
// printf("Robot moves forward\r\n");
// }
// }
}
}
void zoneHandler(int* minorStep, Target obj)
{
if (obj.flag == 0)
{
(*minorStep)--;
moveStraightGrabbing();
usleep(3000000); //delay 3sec
moveStop();
}
}
int lets_Go(int type, VideoCopy *image)
{
int stop_Flag = 0;
clock_t start, last;
start = clock();
// 차면서 앞으로 가기시작
//go_Plastic();
moveStraight();
while(stop_Flag == 0)
{
// 동완이 함수호출
int area = search_plastic(image);
if(area < 20) // 파란색이 안보인다면
stop_Flag = 2; // 안보여서 정지, 예외처리
else if(area > 500)
{
stop_Flag = 3; // 목적지 도착
}
if(type == 1) // 처음부터 중간까지 의무걸음 이라면
{
last = clock();
double time = (double)(last - start)/CLOCKS_PER_SEC;
if(time > 15)
stop_Flag = 1; // 의무걸음 종료
}
}
moveStop();
usleep(200000);
return stop_Flag;
}
void loop()
{
delay(500);
servo.write(90); //Rotate servo to face front
scan(); //Go to scan function
FrontDistance = distance; //FrontDistance =distance returned from scan function
if(FrontDistance > 40 || FrontDistance == 0)
{
moveForward(); //Go to moveForward function
}
else
{
moveStop(); //Go to moveStop function
servo.write(167);
delay(500);
scan(); //Go to scan function
LeftDistance = distance;
servo.write(0);
delay(500);
scan(); //Go to scan function
RightDistance = distance;
if(RightDistance < LeftDistance)
{
moveLeft();
delay(500);
}
else if(LeftDistance < RightDistance)
{
moveRight();
delay(500);
}
else
{
moveBackward();
delay(200);
moveRight();
delay(200);
}
}
}
void approach2Zone(VideoCopy *image, Target obj)
{
ColorBoundary color_B = classifyObject(obj.flag);
int zoneCnt = 0;
int isFind;
while(1)
{
//search
isFind = toleranceZoneSearch(image, &obj, color_B, &zoneCnt);
printf("zone find : %d\n" , isFind);
if(isFind <=0)
break;
else
{
int wPix = (MAX_X/2 - obj.x_point);
printf("wPix : %d\n", wPix);
int direction = wPix < 0 ? 0 : 1;
wPix = abs(wPix);
if(wPix > 60){
if(direction == 0)
moveLeftGrabbing();
else if(direction == 1)
moveRightGrabbing();
}
else{
moveStraightGrabbing();
}
}
}
usleep(5000000); // 경험적 의무걸음
moveStop();
}
void Monster::initParam()
{
countGun = 0;
jumpCount = 0;
speed = 1;
spring = 5;
moveState = MOVE_STOP;
//设置朝向
this->setDir(DIR_RIGHT);
//物体的类型
this->setType(TYPE_MONSTER);
//会和哪些物体方式碰撞
this->setMask(
TYPE_BRICK |
TYPE_COIN |
TYPE_HERO |
TYPE_PRIZE |
TYPE_ARTICLE |
TYPE_WEAPON);
//触动停止动画
moveStop();
}
void QtGradientStopsModel::flipAll()
{
QMap<qreal, QtGradientStop *> stopsMap = stops();
QMapIterator<qreal, QtGradientStop *> itStop(stopsMap);
itStop.toBack();
QMap<QtGradientStop *, bool> swappedList;
while (itStop.hasPrevious()) {
itStop.previous();
QtGradientStop *stop = itStop.value();
if (swappedList.contains(stop))
continue;
const double newPos = 1.0 - itStop.key();
if (stopsMap.contains(newPos)) {
QtGradientStop *swapped = stopsMap.value(newPos);
swappedList[swapped] = true;
swapStops(stop, swapped);
} else {
moveStop(stop, newPos);
}
}
}
void QtGradientStopsModel::moveStops(double newPosition)
{
QtGradientStop *current = currentStop();
if (!current)
return;
double newPos = newPosition;
if (newPos > 1)
newPos = 1;
else if (newPos < 0)
newPos = 0;
if (newPos == current->position())
return;
double offset = newPos - current->position();
QtGradientStop *first = firstSelected();
QtGradientStop *last = lastSelected();
if (first && last) { // multiselection
double maxOffset = 1.0 - last->position();
double minOffset = -first->position();
if (offset > maxOffset)
offset = maxOffset;
else if (offset < minOffset)
offset = minOffset;
}
if (offset == 0)
return;
bool forward = (offset > 0) ? false : true;
PositionStopMap stopList;
QList<QtGradientStop *> selected = selectedStops();
QListIterator<QtGradientStop *> it(selected);
while (it.hasNext()) {
QtGradientStop *stop = it.next();
stopList[stop->position()] = stop;
}
stopList[current->position()] = current;
PositionStopMap::ConstIterator itStop = forward ? stopList.constBegin() : stopList.constEnd();
while (itStop != (forward ? stopList.constEnd() : stopList.constBegin())) {
if (!forward)
--itStop;
QtGradientStop *stop = itStop.value();
double pos = stop->position() + offset;
if (pos > 1)
pos = 1;
if (pos < 0)
pos = 0;
if (current == stop)
pos = newPos;
QtGradientStop *oldStop = at(pos);
if (oldStop && !stopList.values().contains(oldStop))
removeStop(oldStop);
moveStop(stop, pos);
if (forward)
++itStop;
}
}
void GradientStopEditor::mouseMoveEvent( QMouseEvent * event )
{
if( currentMovedStop != -1 )
moveStop( currentMovedStop, event->pos() );
}
void set_Position3(int gathered_Type, int obj_Flag)
{
//while(1){
// 세부조정 루프
printf("see down\n");
// 내려다 보기
seeDown();
usleep(200000); // delay 1sec, camera buffer update
// 내려다 본 결과로 판단
Target obj = findTarget(&bufCopy, obj_Flag);
printf("x : %d, y : %d, flag : %d\n", obj.x_point, obj.y_point, obj.flag);
int up = 0;
int leftRight = 0, front = 0;
// 못 찾았을 경우에 대한 예외처리가 없음 현재는
if(obj.flag == obj_Flag){
if(obj.x_point < detail_minX_Outside)
{
printf("see up\n");
// 몸을 다시 일으키고
seeUp();
usleep(200000);
up++;
rightOneStep();
usleep(200000);
leftRight += 2;
}
else if(obj.x_point > detail_maxX_Outside)
{
printf("see up\n");
// 몸을 다시 일으키고
seeUp();
usleep(200000);
up++;
leftOneStep();
usleep(200000);
leftRight += -2;
}
if(obj.y_point > detail_Y)
{
if(up == 0){
printf("see up\n");
// 몸을 다시 일으키고
seeUp();
usleep(200000);
}
frontOneStep();
usleep(200000);
front++;
}
gathered_Touch(gathered_Type);
usleep(400000);
//break;
}
else
{
int isFind = 0;
// 왼
if(isFind == 0){
leftSee();
usleep(200000);
obj = findTarget(&bufCopy, obj_Flag);
isFind = 1;
if(obj.flag == obj_Flag)
{
seeUp();
usleep(200000);
moveStop();
usleep(200000);
leftOneStep();
usleep(200000);
leftOneStep();
usleep(200000);
leftOneStep();
usleep(200000);
set_Position3(NULL, obj_Flag);
}
}
// 오른
if(isFind == 0){
rightSee();
usleep(200000);
obj = findTarget(&bufCopy, obj_Flag);
isFind = 3;
if(obj.flag == obj_Flag)
{
seeUp();
usleep(200000);
moveStop();
usleep(200000);
rightOneStep();
usleep(200000);
rightOneStep();
usleep(200000);
rightOneStep();
usleep(200000);
set_Position3(NULL, obj_Flag);
}
}
// 뒤로
if(isFind == 0)
{
seeUp();
usleep(200000);
moveStop();
usleep(200000);
go_Backward();
usleep(3000000);
moveStop();
usleep(200000);
set_Position3(NULL, obj_Flag);
}
}
//}
}
void searchZone_n_Kick(VideoCopy *image, Target target)
{
int kickCnt = 0;
int i,j;
int sideFlag_Cnt = 3; // 사이드인지 아닌지 판별할 플래그의 카운트 기준수
while(1)
{
int kickFlag = 0;
Target zone = findZone(image, target);
printf("zone flag : %d\n", zone.flag);
printf("x : %d\n", zone.x_point);
if(zone.flag == target.flag)
{
// 가운데 있다고 판단 되면
if(zone.x_point < maxX && zone.x_point >minX)
{
kickFlag = 1;
printf("kickFlag = 1\n");
}
else if(zone.x_point <=minX)
{
for(i=0; i<1; i++){
rotateOnRight();
usleep(200000);
turnCnt--;
}
}
else if(zone.x_point >=maxX)
{
for(i=0;i<1; i++){
rotateOnLeft();
usleep(200000);
turnCnt++;
}
}
}
else if(zone.flag == 4) // see left
{
for(i=0;i<1; i++){
rotateOnLeft();
usleep(200000);
turnCnt++;
}
}
else if(zone.flag == 5) // see right
{
for(i=0; i<1; i++){
rotateOnRight();
usleep(200000);
turnCnt--;
}
}
else if(zone.flag == 6)
{
printf("zone not found\n");
}
// 세부조정
while(1){
printf("see down\n");
// 내려다 보기
seeDown();
usleep(200000); // delay 1sec, camera buffer update
// 내려다 본 결과로 판단
Target obj = findTarget(&bufCopy, target.flag);
printf("x : %d, y : %d, flag : %d\n", obj.x_point, obj.y_point, obj.flag);
printf("see up\n");
// 몸을 다시 일으키고
seeUp();
usleep(200000);
// 못 찾았을 경우에 대한 예외처리가 없음 현재는
if(obj.flag == target.flag){
if(obj.x_point < detail_minX)
{
rightOneStep();
usleep(200000);
}
else if(obj.x_point >detail_maxX)
{
leftOneStep();
usleep(200000);
}
else if(obj.y_point > detail_Y)
{
frontOneStep();
usleep(200000);
}
else
{
/*
if(kickFlag == 0){
gathered_Touch(4); // 아무의미없는 인자
usleep(200000);
}
*/
break;
}
}
}
// 킥 플래그 서 있으면 찬다
if(kickFlag == 1)
{
gathered_Touch(PLASTIC);
usleep(200000);
left_Kick();
usleep(200000);
// 물체앞까지 의무걸음
moveStraight();
usleep(5500000);
moveStop();
usleep(200000);
kickCnt++;
if(plastic_Start_Pos == 2)
{
if(kickCnt >= side_Cnt)
break;
}
else if(plastic_Start_Pos == 1)
{
if(kickCnt >= straight_Cnt)
break;
}
}
if(kickCnt == 1)
{
if(abs(turnCnt) >= sideFlag_Cnt)
{
plastic_Start_Pos = 2; // 먼 곳에서 시작
}
else
{
plastic_Start_Pos = 1; // 가까운곳, 즉 가운데서 시작
}
}
}
}
void plastic_Turning()
{
int i;
if(turnCnt <= -1) // 내가 왼쪽에서 시작했다고 판단
{
for(i=0; i <4; i++)
{
turn_Right();
usleep(200000);
}
// turn_Right_60();
// usleep(200000);
go_Plastic();
usleep(2700000); //
moveStop();
usleep(200000);
}
else if(turnCnt >= 1) // 내가 오른쪽에서 시작했다고 판단
{
for(i=0; i < 4; i++)
{
turn_Left();
usleep(200000);
}
// turn_Left_60();
// usleep(200000);
go_Plastic();
usleep(2700000); //
moveStop();
usleep(200000);
}
else
{
music_D();
for(i=0; i < 2; i++)
{
turn_Left_90();
usleep(500000);
}
go_Plastic();
usleep(3200000); //
moveStop();
usleep(200000);
}
Target second_Plastic;
int isfind = 0;
// 가운데라면
if(turnCnt == 0)
{
isfind = left_front_right_search(PLASTIC);
if(isfind == 1 || isfind == 5)
{
go_Plastic();
usleep(1000000);
moveStop();
usleep(200000);
}
}
else if(turnCnt <0)
{
isfind = left_front_right_search(PLASTIC);
if(isfind ==0){
turn_Left_45();
usleep(200000);
go_Plastic();
usleep(1000000);
moveStop();
usleep(200000);
int isfind2 = left_front_right_search(PLASTIC);;
if(isfind2 > 0)
{
isfind = 4;
}
}
}
else if(turnCnt >0)
{
isfind = left_front_right_search(PLASTIC);
if(isfind ==0){
turn_Right_45();
usleep(200000);
go_Plastic();
usleep(1000000);
moveStop();
usleep(200000);
int isfind2 = left_front_right_search(PLASTIC);;
if(isfind2 > 0)
{
isfind = 4;
}
}
}
if(isfind == 0)
{
left_Kick();
}
}
void manual_mode()
{
int discardSensors = 1;
while(1) {
_delay_ms(1);
//printf("\r\nPlease enter the input in the format : <dir> <l1><l0> <r1><r0>\r\n");
//char dir = getchar();
if( bit_is_set(UCSR0A, RXC0) != 0) {
char dir = UDR0;
printf("UDR0 = %c\r\n",dir);
if(dir == 'a') {
printf("Auto mode enabled\r\n");
auto_mode();
}
while( (dir !='d') & (dir !='h') & (dir !='f') & (dir !='b') & (dir !='l') & (dir !='r') & (dir !='s') ) {
printf("Enter valid direction character(f,b,l,r,s,d,h)");
char dir = getchar();
}
if(dir=='d') {
discardSensors = 1; // Discard Sensor Values
}
if(dir=='h') {
discardSensors = 0; // Hold Sensor Values
}
if(dir=='f') {
int speedl1 = getchar();
int speedl0 = getchar();
int speedr1 = getchar();
int speedr0 = getchar();
printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
int speedl = ((speedl1-48)*10 + (speedl0-48))*255/99;
int speedr = ((speedr1-48)*10 + (speedr0-48))*255/99;
moveForward(speedl,speedr);
printf("Robot moves forward\r\n");
}
if(dir=='b') {
int speedl1 = getchar();
int speedl0 = getchar();
int speedr1 = getchar();
int speedr0 = getchar();
printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
int speedl = ((speedl1-48)*10 + (speedl0-48))*255/99;
int speedr = ((speedr1-48)*10 + (speedr0-48))*255/99;
moveBackward(speedl,speedr);
printf("Robot moves backward\r\n");
}
if(dir=='r') {
int speedl1 = getchar();
int speedl0 = getchar();
int speedr1 = getchar();
int speedr0 = getchar();
printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
int speedl = ((speedl1-48)*10 + (speedl0-48))*255/99;
int speedr = ((speedr1-48)*10 + (speedr0-48))*255/99;
moveRight(speedl,speedr);
printf("Robot moves right\r\n");
}
if(dir=='l') {
int speedl1 = getchar();
int speedl0 = getchar();
int speedr1 = getchar();
int speedr0 = getchar();
printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
int speedl = ((speedl1-48)*10 + (speedl0-48))*255/99;
int speedr = ((speedr1-48)*10 + (speedr0-48))*255/99;
moveLeft(speedl,speedr);
printf("Robot moves left\r\n");
}
if(dir=='s') {
printf("You entered input as : %c\r\n", dir);
moveStop();
printf("Robot stops\r\n");
}
}
else if( discardSensors!=1 ) {
_delay_ms(50);
int adc_val0=ReadADC(0);
_delay_ms(50);
int adc_val1=ReadADC(1);
_delay_ms(50);
int adc_val2=ReadADC(2);
printf("Sensor Readings : ClifL = %d, ClifR = %d, Bump = %d\r\n",adc_val0,adc_val1,adc_val2);
if( (adc_val0 < 100) ) {
//if( (adc_val0 < 100) | (adc_val1 < 100) | (adc_val2 < 100) ) {
moveStop();
printf("Robot stops\r\n");
_delay_ms(0500);
moveBackward(160,160);
printf("Robot moves backward\r\n");
_delay_ms(1000);
moveStop();
printf("Robot stops\r\n");
_delay_ms(0500);
moveRight(160,160);
printf("Robot moves Right\r\n");
_delay_ms(0500);
moveStop();
printf("Robot stops\r\n");
_delay_ms(0500);
}
}
}
}
int left_front_right_search(int flag)
{
int isfind = 0;
Target obj;
if(isfind ==0){
obj = findTarget(&bufCopy, flag);
if(obj.flag == flag)
{
isfind = 3;
}
}
if(isfind ==0){
leftSee();
usleep(200000);
obj = findTarget(&bufCopy, flag);
if(obj.flag == flag)
{
isfind = 2;
turn_Left_45();
usleep(200000);
//go_Plastic();
//usleep(1000000);
//moveStop();
//usleep(200000);
}
}
if(isfind ==0){
leftSee_80();
usleep(200000);
obj = findTarget(&bufCopy, flag);
if(obj.flag == flag)
{
isfind = 1;
turn_Left_90();
usleep(200000);
//go_Plastic();
//usleep(1000000);
//moveStop();
//usleep(200000);
}
}
if(isfind ==0){
rightSee();
usleep(200000);
obj = findTarget(&bufCopy, flag);
if(obj.flag == flag)
{
isfind = 4;
turn_Right_45();
usleep(200000);
//go_Plastic();
//usleep(1000000);
//moveStop();
//usleep(200000);
}
}
if(isfind ==0){
rightSee_80();
usleep(200000);
obj = findTarget(&bufCopy, flag);
if(obj.flag == flag)
{
isfind = 5;
turn_Right_90();
usleep(200000);
//go_Plastic();
//usleep(1000000);
//moveStop();
//usleep(200000);
}
}
moveStop();
usleep(200000);
return isfind;
}
int plastic_Turning2()
{
int stop_Flag = 0;
Target redZone;
while(1){
redZone = findTarget(&bufCopy, COKE);
if(redZone.flag == 0)
{
turn_Left_Grabbing();
}
else
{
break;
}
}
redZone = findTarget(&bufCopy, COKE);
ColorBoundary color_B = classifyObject(COKE);
int zoneCnt = 0;
int isFind;
int cnt = 0;
int first_Flag = 1;
while(1)
{
//search
isFind = toleranceZoneSearch(&bufCopy, &redZone, color_B, &zoneCnt);
printf("zone find : %d\n" , isFind);
if(isFind <=0)
{
// cnt++;
// moveStraightGrabbing();
// if(cnt > 2)
break;
}
else
{
int wPix = (MAX_X/2 - redZone.x_point);
printf("wPix : %d\n", wPix);
int direction = wPix < 0 ? 0 : 1;
wPix = abs(wPix);
if(wPix > 60){
if(direction == 0)
go_Plastic_Left();
else if(direction == 1)
go_Plastic_Right();
}
else{
go_Plastic();
}
first_Flag = 0 ; // 디버깅용 코드
}
}
if(first_Flag == 1){ // 디버깅용 코드
moveStop();
usleep(200000);
left_Kick();
}
moveStop();
}
Target zone_Search_Handler(VideoCopy *image, Target obj)
{
Target zone;
redZoneCnt = blueZoneCnt = greenZoneCnt = 0;
while(1){
zone = findZone(image, obj);
printf("obj flag : %d\n", zone.flag);
if(zone.flag == MILK)
{
if(zone.x_point < 50)
turn_Right_Grabbing();
else if(zone.x_point > 270)
turn_Left_Grabbing();
break;
}
else if(zone.flag == 7) // red
{
turn_Right_Grabbing();
usleep(200000);
redZoneCnt++;
}
else if(zone.flag == 8) // blue
{
turn_Right_Grabbing();
usleep(200000);
blueZoneCnt++;
}
else if(zone.flag > 3)
{
printf("zone not found\n");
right_Kick();
usleep(200000);
}
}
if(redZoneCnt > 0){
moveStraightGrabbing();
usleep(15000000);
moveStop();
usleep(200000);
}
/*
else if(blueZoneCnt > 0){
moveStraightGrabbing();
usleep(5000000);
moveStop();
usleep(200000);
}
*/
// while(1){
zone = findZone(image, obj);
printf("obj flag : %d\n", zone.flag);
if(zone.flag == MILK)
{
if(zone.x_point < 50)
turn_Right_Grabbing();
else if(zone.x_point > 270)
turn_Left_Grabbing();
// break;
}
else if(zone.flag == 7) // red
{
turn_Right_Grabbing();
usleep(200000);
redZoneCnt++;
}
else if(zone.flag == 8) // blue
{
turn_Right_Grabbing();
usleep(200000);
blueZoneCnt++;
}
else if(zone.flag > 3)
{
printf("zone not found\n");
// turn_Left_Grabbing();
usleep(200000);
}
// }
return zone;
}
void approach2Zone(VideoCopy *image, Target obj)
{
ColorBoundary color_B = classifyObject(obj.flag);
int zoneCnt = 0;
int isFind;
int cnt = 0;
int first_Flag = 1;
while(1)
{
//search
isFind = toleranceZoneSearch(image, &obj, color_B, &zoneCnt);
printf("zone find : %d\n" , isFind);
if(isFind <=0)
{
// cnt++;
// moveStraightGrabbing();
// if(cnt > 2)
break;
}
else
{
int wPix = (MAX_X/2 - obj.x_point);
printf("wPix : %d\n", wPix);
int direction = wPix < 0 ? 0 : 1;
// int direction = wPix < 0 ? 1 : 0;
wPix = abs(wPix);
if(wPix > 60){
if(direction == 0)
moveLeftGrabbing();
else if(direction == 1)
moveRightGrabbing();
}
else{
moveStraightGrabbing();
}
first_Flag = 0 ; // 디버깅용 코드
}
}
if(first_Flag == 1){ // 디버깅용 코드
moveStop();
usleep(200000);
putTarget();
left_Kick();
}
if(redZoneCnt > 0)
{
usleep(7000000); // 경험적 의무걸음
}
else
{
usleep(6000000);
}
moveStop();
}
int GradientStopEditor::moveStop( int stop, const QPointF &newPos )
{
return moveStop( stop, 255 - int( ( qreal( newPos.y() ) / qreal( height() - 1 ) ) * 255.0 ),
qreal( newPos.x() ) / qreal( width() - 1 ) );
}
bool MapHelperUniversal::keyPress(QKeyEvent *event)
{
Q_UNUSED(event);
#if 0
Q_D(MapHelperUniversal);
switch (event->key()) {
case Qt::Key_Q: {
if (Qt::NoModifier == event->modifiers()) {
move();
qDebug() << "Move On";
}
else if (Qt::ShiftModifier == event->modifiers()) {
moveStop();
qDebug() << "Move Off";
}
break;
}
case Qt::Key_W: {
if (Qt::NoModifier == event->modifiers()) {
rotate();
qDebug() << "Rotate On";
}
else if (Qt::ShiftModifier == event->modifiers()) {
rotateStop();
qDebug() << "Rotate Off";
}
break;
}
case Qt::Key_E: {
if (Qt::NoModifier == event->modifiers()) {
scale();
qDebug() << "Scale On";
}
else if (Qt::ShiftModifier == event->modifiers()) {
scaleStop();
qDebug() << "Scalse Off";
}
break;
}
case Qt::Key_R: {
if (Qt::NoModifier == event->modifiers()) {
select();
qDebug() << "Select On";
}
else if (Qt::ShiftModifier == event->modifiers()) {
selectStop();
qDebug() << "Select Off";
}
break;
}
case Qt::Key_T: {
if (Qt::NoModifier == event->modifiers()) {
edit();
qDebug() << "Edit On";
}
else if (Qt::ShiftModifier == event->modifiers()) {
editStop();
qDebug() << "Edit Off";
}
break;
}
case Qt::Key_Y: {
if (Qt::NoModifier == event->modifiers()) {
create();
qDebug() << "Create On";
}
else if (Qt::ShiftModifier == event->modifiers()) {
createStop();
qDebug() << "Create Off";
}
break;
}
case Qt::Key_A: {
qDebug() << "Custom No On";
d->customAction = 0;
break;
}
case Qt::Key_S: {
qDebug() << "Custom Move On";
d->customAction = 1;
break;
}
case Qt::Key_D: {
qDebug() << "Custom Select On";
d->customAction = 2;
break;
}
case Qt::Key_F: {
qDebug() << "Custom Edit On";
d->customAction = 4;
break;
}
case Qt::Key_G: {
qDebug() << "Custom Create On";
d->customAction = 3;
break;
}
default:
qDebug() << event;
break;
}
QString types = "";
foreach (MapSubHelper *sub, d->subhelpers)
types += QString::number(sub->type()) + " ";
qDebug() << types;
#endif
return true;
}
void milk_Turning()
{
// 양수는 오른쪽으로 회전했었다
// 음수는 왼쪽
int i;
if(redZoneCnt > 0)
{
/*for(i=0; i <3; i++)
{
turn_Right();
usleep(200000);
}*/
turn_Right_60();
usleep(200000);
turn_Right_45();
usleep(200000);
go_Plastic();
usleep(3800000);
}
else if(blueZoneCnt > 0)
{
for(i=0; i <3; i++)
{
turn_Right_45();
usleep(500000);
}
go_Plastic();
usleep(3500000);
}
else
{
for(i=0; i <2; i++)
{
turn_Right_90();
usleep(500000);
}
go_Plastic();
usleep(3000000);
}
// 의무 보행하여 가운데쪽으로 좀 나오자
//moveStraight();
//usleep(8000000); // 15 걸음 의도
moveStop();
usleep(200000);
/*
for(i=0; i <2; i++)
{
turn_Left();
usleep(200000);
}
// 의무 보행하여 가운데쪽으로 좀 나오자
moveStraight();
usleep(4000000); //
moveStop();
usleep(200000);
*/
// 여기서부터 재탐색
int isFind = 0;
isFind = left_front_right_search(MILK);
if(isFind == 0)
{
turn_Right();
usleep(200000);
turn_Right();
usleep(200000);
go_Plastic();
usleep(2000000);
moveStop();
usleep(200000);
isFind = left_front_right_search(MILK);
}
if(isFind == 0)
{
turn_Right();
usleep(200000);
go_Plastic();
usleep(2000000);
moveStop();
usleep(200000);
isFind = left_front_right_search(MILK);
}
}
