int find_item()
{
camera_update();
point2 mcenter = get_object_center(1,0);
set_servo_position (0 ,400);
while(get_object_count(1) == 0){
camera_update();
create_spin_CCW(25);
}
create_stop();
sleep(1);
create_spin_CW(15);
msleep(100);
while(mcenter.x < 70 || mcenter.x > 100){
camera_update();
mcenter = get_object_center(1,0);
if(mcenter.x < 70){
create_spin_CCW(15);
camera_update();
mcenter = get_object_center(1,0);
}
if(mcenter.x < 100){
create_spin_CW(15);
camera_update();
mcenter = get_object_center(1,0);
}
if(mcenter.x > 70 && mcenter.x < 100){
create_stop();
}
}
beep();
}
void sort_balls(void) {
start_spinner();
while (!side_button()) {
msleep(50);
camera_update(); camera_update();
if (get_object_count(kCameraChannelOrange) > 0 && get_object_confidence(kCameraChannelOrange, 0) >= 0.95 && get_object_area(kCameraChannelOrange, 0) > 500) {
stop_spinner();
set_servo_position(kServoPortSorter, kServoPositionSorterOrange);
msleep(200);
set_servo_position(kServoPortSorter, kServoPositionSorterCenter);
msleep(200);
start_spinner();
} else if (get_object_count(kCameraChannelGreen) > 0 && get_object_confidence(kCameraChannelGreen, 0) >= 0.95 && get_object_area(kCameraChannelGreen, 0) > 500) {
stop_spinner();
set_servo_position(kServoPortSorter, kServoPositionSorterGreen);
msleep(200);
set_servo_position(kServoPortSorter, kServoPositionSorterCenter);
msleep(200);
start_spinner();
}
}
alloff();
}
inline void blob_update()
{
// set config file
camera_update();
//update green state
current.green.x = get_object_center(0 , 0).x;
current.green.y = get_object_center(0 , 0).y;
current.green.size = get_object_area(0 , 0);
//update orange state
current.orange.x = get_object_center(1 , 0).x;
current.orange.y = get_object_center(1 , 0).y;
current.orange.size = get_object_area(1 , 0);
//update red state
current.red.x = get_object_center(2 , 0).x;
current.red.y = get_object_center(2 , 0).y;
current.red.size = get_object_area(2 , 0);
// update botguy state
current.botguy.x = get_object_center(3 , 0).x;
current.botguy.y = get_object_center(3 , 0).y;
current.botguy.size = get_object_area(3 , 0);
// set config file
camera_update();
// update pink booster state
current.pink.x = get_object_center(0 , 0).x;
current.pink.y = get_object_center(0 , 0).y;
current.pink.size = get_object_area(0 , 0);
// update teal booster state
current.teal.x = get_object_center(1 , 0).x;
current.teal.y = get_object_center(1 , 0).y;
current.teal.size = get_object_area(1 , 0);
}
// prepare:
void prepare(){
camera_open(LOW_RES);
camera_load_config(CONFIG);
camera_update();
camera_update();
camera_update();
// prepare camera
enable_servos();
set_servo_position(BSV, BSV_LEVEL);
set_servo_position(ASV, ASV_BACK);
set_servo_position(RSV, RSV_DOWN);
set_servo_position(SSV, SSV_BACK);
// prepare servos
set_analog_pullup(FSS, 0);
set_analog_pullup(BSS, 0);
// prepare sensors
display_clear();
printf("prepare succeeds");
while(digital(TSS) == 0);
msleep(1500);
//wait_for_light(SSS);
//shut_down_in(TIME_LIMIT);
// show controller that coke is ready
}
void cube_is_near(){
camera_update();
msleep(500);
camera_update();
enable_servos();
claw_open();
printf("cube is near");
freeze(Motor_Left);
freeze(Motor_Right);
if(get_object_count(0)!=0){
printf("gleich gefunden\n");
if(get_object_bbox(0,0).width > 20){
printf("groß genug");
funden=1;
found_something();
}
}
double s=seconds();
motor(Motor_Right,30);
while(funden==0){//nichts gefunden
camera_update();
printf("nach rechts\n");
if(get_object_count!=0){
printf("1 gefunden \n");
if(get_object_bbox(0,0).width > 20){
printf("groß genug");
found_something();
funden=1;
break;
}
}
if(seconds()>s+2){
printf("zeit rechts\n");
freeze(Motor_Right);
break;
}
}
s=seconds();
motor(Motor_Left,30);
while(funden==0){
camera_update();
printf("nach links drehen \n");
if(get_object_count!=0){
printf("1 gefunden \n");
if(get_object_bbox(0,0).width > 20){
printf("groß genug");
found_something();
funden=1;
break;
}
}
if(seconds()>s+4){
printf("zeit links\n");
freeze(Motor_Left);
break;
}
}
}
void findBall(int color)
{
camera_update();
display_clear();
int count = 1;
int objNum = get_object_count(color);
while(objNum == 0)
{
camera_update();
objNum = get_object_count(color);
}
printf("%d time finding a ball", count);
point2 objCen = get_object_center(color, 0);
int errorX = 0, errorY = 0;
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
while(!(a_button_clicked()))
{
if(BALL_NUM_BOX >= 2)
break;
if(errorX > -4 && errorX < 4 && errorY > -4 && errorY < 4)
break;
int turnLM = -1 * errorX * P_X + errorY * P_Y;
int turnRM = errorX * P_X + errorY * P_Y;
if(turnLM > -1 * MINS && turnLM < 0)
turnLM = -1 * MINS;
if(turnLM > 0 && turnLM < MINS)
turnLM = MINS;
if(turnRM > -1 * MINS && turnRM < 0)
turnRM = -1 * MINS;
if(turnRM > 0 && turnRM < MINS)
turnRM = MINS;
motor(LM, turnLM);
motor(RM, turnRM);
camera_update();
int objArea = get_object_area(color, 0);
while(objArea < 200)
{
ao();
camera_update();
objArea = get_object_area(color, 0);
}
count++;
display_clear();
printf("%d time finding a ball ", count);
printf("%d NUM", BALL_NUM_BOX);
objCen = get_object_center(color, 0);
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
}
ao();
catchBalls(color);
}
void moveToBalls(){
camera_update();
camera_update();
camera_update();
camera_update();
int objNum = get_object_count(GREEN);
while(objNum == 0){
motor(LM, 83);
motor(RM, 100);
camera_update();
objNum = get_object_count(GREEN);
}
msleep(250);
}// move forward; stop when sees a target ball
int main()
{
int lspeed = 10;
int hspeed = 80;
camera_open(LOW_RES);
while (side_button() == 0)
{
camera_update();
if (get_object_center(0 , 0).x < 45)
{
motor(1 , lspeed);
motor(3 , hspeed);
printf("LEFT\n");
}
if (get_object_center(0 , 0).x > 115)
{
motor(1 , hspeed);
motor(3 , lspeed);
printf("RIGHT\n");
}
if (get_object_center(0 , 0).x >= 45 && get_object_center(0 , 0).x <= 115)
{
motor(1 , hspeed);
motor(3 , hspeed);
printf("CENTER\n");
}
}
return 0;
}
void seal_update() {
struct timeval now;
gettimeofday(&now, NULL);
// delta只计算draw的时间
float dt = ((now.tv_sec - _lastUpdate.tv_sec) +
(now.tv_usec - _lastUpdate.tv_usec))/1000000.0f;
_lastUpdate = now;
if (dt < FLT_EPSILON) {
return;
}
GAME->global_dt = dt;
camera_update(GAME->global_camera);
lua_State* L = GAME->lstate;
lua_pushvalue(L, UPDATE_FUNC_INDEX);
lua_pushnumber(L, dt);
seal_call(L, 1, 0);
lua_settop(L, TOP_FUNC_INDEX);
}
int check_stopping_event(int stopping_event) {
if (stopping_event == STOP_BY_ET) {
if (analog_et(ET_SENSOR) < 300) {
return 1;
}
} else if (stopping_event == STOP_BY_TOPHAT) {
display_printf(0, 0, "%4i", analog(L_TOPHAT));
if (analog(L_TOPHAT) > 750 || analog(R_TOPHAT) > 750) {
return 1;
}
} else if (stopping_event == STOP_BY_CAMERA) {
camera_update();
if (get_object_count(0) < 1) {
display_printf(0, 0, "No Object Found");
return 0;
}
if (get_object_area(0, 0) > PINGPONG_THRESHOLD) {
display_clear();
display_printf(0, 0, "Object Seen!");
if (get_object_center(0, 0).x > 90 && get_object_center(0, 0).x < 110) {
display_printf(0, 1, "Object centered");
return 1;
}
}
} else {
printf("Stopping type is not defined!\n");
return -1;
}
return 0;
}
//routine to use the camera and see blob position based on standard create start routine
void calibrateCamera() {
int channel = 0;
while (a_button_clicked() == 0) {
msleep(2000);
camera_update();
printf ("=> object count %d \n", get_object_count(channel));
int i = 0;
int shelfPlaces[8] = {0,0,0,0,0,0,0,0};
while (i < get_object_count(channel)) {
printf("obj %d, ctr loc, area, shelf plcmt: %d, %d, %d\n", i, get_object_center(channel, i).x, get_object_area(channel, i),
getShelfPlacement(get_object_center(channel, i).x));
shelfPlaces[getShelfPlacement(get_object_center(channel, i).x)] += get_object_area(channel, i);
i++;
}
printf("==========================\n");
printf("=RESULTS =\n");
printf("==========================\n");
i = 1;
while (i < 8) {
printf ("shelfPlace : %d, total cube area : %d\n", i, shelfPlaces[i]);
i++;
}
printf("==========================\n");
printf("click b button for new camera capture, a to exit.\n");
while (b_button_clicked() == 0 && a_button_clicked() == 0 ) {
msleep(25);
}
}
printf("done with calibration function.\n");
}
int main() {
Button button;
int status;
start_buttons();
status = camera_open();
if (status == FALSE) {
printf("Could not open the camera.\n");
return 1;
}
while (TRUE) {
button = button_pressed();
switch (button) {
case A_BUTTON: cv_show_image(RAW_IMAGE); break;
case B_BUTTON: cv_show_image(GRAYSCALE); break;
case C_BUTTON: cv_show_image(CANNY); break;
case X_BUTTON: cv_show_image(HOUGHLINES); break;
case Y_BUTTON: camera_update(); break;
case Z_BUTTON: cv_show_image(RAW_IMAGE); break;
}
}
camera_close();
return 0;
}
/*
* Update game variables
*/
void update(void)
{
// Calculate the elapsed time between frames
double current_time = Wallclock();
double delta_time = current_time - g_dLastTime;
g_dLastTime = current_time;
// Update if the game isn't paused
if( !g_bPause )
{
// Update player movement
player_update(delta_time);
// Update jeep movement
g_pModelJeep.rotation += 0.05;
g_pModelJeep.position[0] = sin(ToRad(g_pModelJeep.rotation)) * 20.0f;
g_pModelJeep.position[1] = cos(ToRad(g_pModelJeep.rotation)) * 20.0f;
// Update camera movement
camera_update(delta_time, player_getRotation());
}
// Redraw the scene
glutPostRedisplay();
Sleep(1);
}
//Precondition: camera must already be open
int center_on_blob(int desired_x, int color,float time ) {
int actual_x;
while (1) {
camera_update();
actual_x = get_object_center(color, 0).x;
printf("%i\n");
if (actual_x > desired_x) {
RIGHT(-20);
LEFT(20);
msleep(40);
LFREEZE();
RFREEZE();
}
if (actual_x < desired_x) {
RIGHT(20);
LEFT(-20);
msleep(40);
LFREEZE();
RFREEZE();
}
if (actual_x < desired_x+3 && actual_x > desired_x-3){
LFREEZE();
RFREEZE();
break;
}
msleep(200);
}
}
void resize(int width, int height)
{
screen_size = (vec2f_t) { width, height };
buffer_size = (vec2f_t) { round_up_to_pot(width), round_up_to_pot(height) };
camera.aspect = (float)width/(float)height;
camera.scale = (float)width;
camera_update(&camera);
gfx_resize(gfx, width, height);
gfx_depth_range(gfx, camera.znear, camera.zfar);
vec2f_t rect = { width, height };
char* textures[] = { "test_texture" };
texture_options_t options =
{
TF_LINEAR,
TF_LINEAR,
TW_REPEAT,
TW_REPEAT,
};
uint32_t ntextures = sizeof(textures)/sizeof(textures[0]);
uint32_t i = 0;
for (i = 0; i < ntextures; ++i)
{
gfx_setup_texture(gfx, textures[i], &rect, &options);
}
gfx_setup_target(gfx, "test_target", &rect, (const char**)textures, ntextures);
screen_resize(width, height);
}
void found_something(){
ao();
camera_update();
msleep(100);
camera_update();
msleep(100);
camera_update();
msleep(100);
while(get_object_bbox(0,0).width < 80){
camera_update();
if(get_object_center(0,0).x > 120)
{
motor(Motor_Left,Drivespeed_middle/2);
motor(Motor_Right,-Drivespeed_middle/2);
camera_update();
} else if(get_object_center(0,0).x < 70) {
motor(Motor_Left,-Drivespeed_middle/2);
motor(Motor_Right,Drivespeed_middle/2);
camera_update();
} else {
motor(Motor_Left,Drivespeed_middle/2);
motor(Motor_Right,Drivespeed_middle/2);
camera_update();
}
msleep(5);
}
freeze(Motor_Left);
freeze(Motor_Right);
claw_close();
msleep(1000);
claw_up();
}
int getNumVisiblePingPongBalls(void)
{
//camera_open();
camera_update();
int count = get_object_count(PING_PONG_BALL_CHANNEL_RED);
//camera_close();
return count;
}
int main()
{
camera_open();
camera_update();
graphics_open(get_camera_width(), get_camera_height());
while(!get_key_state('Q'))
{
camera_update();
graphics_blit_enc(get_camera_frame(), BGR, 0, 0, get_camera_width(), get_camera_height());
graphics_circle_fill(get_camera_width() / 2, get_camera_height() / 2, get_camera_height() / 20, 255, 0, 0);
graphics_update();
}
camera_close();
graphics_close();
return 0;
}
void moveToBalls(){
camera_update();
camera_update();
camera_update();
camera_update();
int objNum = get_object_count(GREEN);
while(objNum == 0){
motor(LM, LM_SP_STR1);
motor(RM, RM_SP_STR1);
camera_update();
objNum = get_object_count(GREEN);
}
mrp(LM, -1 * LM_SP_STR2, 120);
mrp(RM, -1 * RM_SP_STR2, 120);
bmd(LM);
bmd(RM);
ao();
//msleep(250);
}// move forward; stop when sees a target ball
// prepare:
void prepare(){
camera_open(LOW_RES);
camera_load_config(CONFIG);
camera_update();
camera_update();
camera_update();
// prepare camera
enable_servos();
set_servo_position(BSV, BSV_LEVEL);
set_servo_position(ASV, ASV_BACK);
set_servo_position(RSV, RSV_DOWN);
set_servo_position(SSV, SSV_BACK);
// prepare servos
display_clear();
printf("prepare succeeds");
while(!(a_button_clicked()));
// show controller that coke is ready
}
// check the ball:
int checkBalls(int color){
camera_open();
camera_update();
if ( get_object_area(color, 0) > 3000 ){
return 0;
}
return 1;
}
void update_camera(double t)
{
IF_FAILED(init);
// зумируем изменяя угол обзора
wheel_rot = input_get_wheel_rot();
render_set_camera_fov(wheel_rot);
camera_update(&camera, t);
}
Coordinate getCoordinateOfPingPongBall(int PingPongBallNumber)
{
//camera_open();
Coordinate cood = *((Coordinate*)malloc(sizeof(Coordinate)));
camera_update();
msleep(100);
cood.x = get_object_center_x(PING_PONG_BALL_CHANNEL,PingPongBallNumber);
cood.y = get_object_center_y(PING_PONG_BALL_CHANNEL,PingPongBallNumber);
//camera_close();
return cood;
}
int main() {
printf("hello\n");
camera_open(LOW_RES);
camera_update();
printf("Area: %i\n", get_object_area(0, 0));
CvMat* x;
Mat y;
return 1;
}
int main ()
{
camera_open();
depth_open();
while(!(a_button_clicked())){
camera_update();
depth_update();
display_clear();
printf("prepare succeeds");
}
return 0;
}
int main(){
armUp();
clawOpen();
camera_open(LOW_RES);
double start_wait=seconds();
while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds
int i=0;
while(i<10){//picks latest image from the buffer
camera_update();
i++;
}
if(get_object_count(chan)>0){
break;
}else{
stop(0.1);
}
}
while((seconds()-start_wait)<=60){
int area=get_object_area(chan, 0);
if(area>=600){
int i=0;
while(i<10){
camera_update();
i++;
}
int x=get_object_center(chan, 0).x;
if(x<65){
rightF(0.1, 100, 80);
}else{
leftF(0.1, 100, 80);
}
}else{
break;
}
}
armDown();
clawClose();
armUp();
}
void bringback2cube(){
//claw up
motor(Motor_Up,Motor_up_speed);
//zurück 1s
drive(1000,-Drivespeed,-Drivespeed);
//turn right 90
drive(970,Turnspeed,-Turnspeed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//back
drive(500,-Drivespeed,-Drivespeed);
//turn so little bit b4 cubes
drive(1130,Turnspeed,-Turnspeed);
//vor
drive(900,Drivespeed,Drivespeed);
//wait for claw up
claw_up();
//camera fix
camera_update();
//hide your cubes
cube_is_near();
//back
drive(1000,-Drivespeed,-Drivespeed);
//turn left more than 90 idk
drive(1330,-Turnspeed,Turnspeed);
//start to down motor
motor(Motor_Up,Motor_down_speed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//langsamer weil wackeldackel
drive(2000,Drivespeed_middle+20,Drivespeed_middle+20);
//back
drive(400,-Drivespeed,-Drivespeed);
//turn more than 90 lulz
drive(980,-Turnspeed,Turnspeed);
//light left and shit
drive(5000,Drivespeed_middle*2,(Drivespeed_middle*2)-7);
//light back and shit
drive(1000,-Drivespeed_middle*2,(-Drivespeed_middle*2)+7);
//wait for claw down
while(!digital(Sensor_Down)){
if(seconds()>start+113){
claw_open();
disable_servos();
}
}
freeze(Motor_Up);
msleep(2000);
claw_open();
disable_servos();
}
int goTowardsObject(int channel){
int allowance = 10; //tweak
int increment = 10; //tweak
//do a circle to identify object.
//turn back and forth in smaller increments until it's in the middle of the image
//go forwards
//recurse
camera_update();
while(areWeThereYet(channel) == 0){
AlignWithObject(channel, allowance);
goStrightInCM(increment);
}
}
void take_position() {
//zurück 2sec
drive(1700,-Drivespeed,-Drivespeed);
//90 nach rechts
drive(950,Turnspeed,-Turnspeed);
//camera fix
camera_update();
//oida drive
drive(2000,Drivespeed,Drivespeed);
//90 nach rechts
drive(925,Turnspeed,-Turnspeed);
drive_till_line();
}
int main(int argc, char* argv[])
{
if(!camera_open()) {
printf("Failed to open camera.\n");
return 1;
}
camera_update();
printf("Closing camera...\n");
camera_close();
return 0;
}
