Новичок
Профиль
Группа: Участник
Сообщений: 5
Регистрация: 3.12.2016
Репутация: нет Всего: нет
|
Нужно изменить функцию user_loop() чтобы определить угол наклона комнаты. В комнате криволинейная поверхность. Определить средний угол наклона комнаты, помогите пожалуста. Код | //robocup.h #ifndef ROBOCUP_H #define ROBOCUP_H void set_movement(float left, float right); void set_servo_angle(float angle); void set_display_text(const char* text); float get_ultrasonic(); int get_grayscale(int no); float get_accel_x(); float get_accel_y(); float get_accel_z(); float get_audio_level(); float get_time(); void delay(int msec); void user_loop(); #define LEFT 0 #define CENTER 1 #define RIGHT 2 #endif // ROBOCUP_H //robocup-api-plain.cpp #ifdef WIN32 #include <WinSock2.h> #include <Windows.h> inline long long gettimeus() { static LARGE_INTEGER ClockPerSecond = { 0 }; if( ClockPerSecond.QuadPart == 0 ) QueryPerformanceFrequency( &ClockPerSecond ); LARGE_INTEGER li; QueryPerformanceCounter( &li ); return li.QuadPart * 1000000LL / ClockPerSecond.QuadPart; } #pragma comment(lib, "ws2_32.lib") #else // WIN32 #include <arpa/inet.h> #include <unistd.h> #include <sys/time.h> inline long long gettimeus() { struct timeval tv; gettimeofday( &tv, NULL ); return (long long) tv.tv_sec * 1000000LL + (long long) tv.tv_usec; } #endif // WIN32 #include <cmath> #include <cstdlib> #include <cstring> #include <cstdio> #include "robocup.h" using namespace std; const int buf_len = 256; int sock; char buf[buf_len]; long long start_time = 0; bool initNet() { sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); sockaddr_in addr = {}; addr.sin_family = AF_INET; addr.sin_port = htons(20037); addr.sin_addr.s_addr = inet_addr("127.0.0.1"); return connect(sock, (sockaddr*)&addr, sizeof(addr)) == 0; } void initRobo() { strcpy(buf, "I0=arduino\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("I> %s\n", buf); strcpy(buf, "ASD=0,0\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("ASD> %s\n", buf); strcpy(buf, "ASO0=0,1\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("ASO0> %s\n", buf); strcpy(buf, "ASO1=0,2\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("ASO1> %s\n", buf); strcpy(buf, "ASO2=0,3\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("ASO2> %s\n", buf); strcpy(buf, "ASG0=0,4\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("ASG0> %s\n", buf); strcpy(buf, "ASG1=0,5\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("ASG1> %s\n", buf); strcpy(buf, "ASG2=0,6\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("ASG2> %s\n", buf); strcpy(buf, "ASA=0,7\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("ASA> %s\n", buf); } void setMotor(int i, double v) { sprintf(buf, "SRD0,%d=%lf\n", i, v); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf("SRD0,%d=%lf> %s\n", i, v, buf); } int main() { #ifdef WIN32 WSADATA wsa; WSAStartup(MAKEWORD(1, 1), &wsa); STARTUPINFOA si = {}; si.cb = sizeof(STARTUPINFOA); PROCESS_INFORMATION pi = {}; CreateProcessA("robo-plain.exe", "", NULL, NULL, FALSE, NORMAL_PRIORITY_CLASS, NULL, NULL, &si, &pi); #else system("./robo-plain >/dev/null &"); //system("ffmpeg -f x11grab -r 30 -s 939x738 -i $DISPLAY+399,29 -vcodec libx264 -threads 0 output.mkv 2>/dev/null >/dev/null &"); #endif delay(2000); bool inited = false;//initNet(); for (int i = 0; i < 5 && !(inited = initNet()); i++) { delay(1); } if (!inited) { fprintf(stderr, "simulation run failed\n"); return 1; } initRobo(); start_time = gettimeus(); while (true) { user_loop(); } return 0; } void set_movement(float left, float right) { if (left < -1) left = -1; if (left > 1) left = 1; if (right < -1) right = -1; if (right > 1) right = 1; setMotor(0, left); setMotor(1, right); } void set_servo_angle(float angle) { if (angle < -90 || angle > 90) return; setMotor(2, angle / 100); } /*void set_display_text(const char *text) { // do nothing here }*/ void set_rgb_led(int red, int green, int blue) { setMotor(3, red <= 55 ? -1 : (red - 155) / 100.0); setMotor(4, green <= 55 ? -1 : (green - 155) / 100.0); setMotor(5, blue <= 55 ? -1 : (blue - 155) / 100.0); } float get_ultrasonic() { sprintf(buf, "GSD\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); return atof(buf)*100; } int get_grayscale(int no) { sprintf(buf, "GSO%d\n", no); //printf("%s", buf); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); //printf(">%s", buf); return 1-atoi(buf); } float get_accel_x() { sprintf(buf, "GSG0\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); return atof(buf); } float get_accel_y() { sprintf(buf, "GSG1\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); return atof(buf); } float get_accel_z() { sprintf(buf, "GSG2\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); return atof(buf); } float get_audio_level() { sprintf(buf, "GSA\n"); send(sock, buf, strlen(buf), 0); recv(sock, buf, buf_len, 0); return atof(buf); } float get_time() { return (gettimeus() - start_time) / 1000000.0f; } void delay(int msec) { #ifdef _WIN32 Sleep(msec); #else usleep(msec * 1000); #endif } //solution.cpp #include "robocup.h" #include <iostream> using namespace std; int phase = 0; float time; void user_loop() { int lgt1 = get_grayscale(1); switch (phase) { case 0: if (lgt1 == 1) { set_movement(1, 1); } else { set_movement(-1, -1); phase = 1; time = get_time(); } break; case 1: if (get_time() - time < 0.5f) { set_movement(-1, -1); } else { set_movement(-1, 1); phase = 2; time = get_time(); } break; case 2: if (get_time() - time < 2.0f) { set_movement(-1, 1); } else { set_movement(1, 1); phase = 0; } break; } }
|
|