Новичок
Профиль
Группа: Участник
Сообщений: 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;
}
}
|
|
Определить угол наклона комнаты - Arduino, Определить средний угол наклона комнаты 
Загрузка ...
