Форум программистов
 

Восстановите пароль или Зарегистрируйтесь на форуме, о проблемах и с заказом рекламы пишите сюда - alarforum@yandex.ru, проверяйте папку спам!

Вернуться   Форум программистов > IT форум > Помощь студентам
Регистрация

Восстановить пароль
Повторная активизация e-mail

Купить рекламу на форуме - 42 тыс руб за месяц

Ответ
 
Опции темы Поиск в этой теме
Старый 08.12.2017, 01:35   #1
drako08
Пользователь
 
Регистрация: 04.10.2016
Сообщений: 30
По умолчанию Определить угол наклона комнаты - Arduino

Нужно изменить функцию 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;
    }
}
drako08 вне форума Ответить с цитированием
Старый 11.12.2017, 08:53   #2
WorldMaster
Старожил
 
Аватар для WorldMaster
 
Регистрация: 25.08.2011
Сообщений: 2,841
По умолчанию

Зачем выкладывать кучу кода без комментариев?? Врядли ктото так прям сходу сможет понять что там и как работает. Если робот то хоть бы фотку показали что это за система и какие принципы в нее заложены.

По коду видно что вы используете данные с акселерометра. Значит можете воспользоваться вот этой техникой для вычисления угла наклона акселерометра.
Или Вот еще статья.

Если датчик не в одной плоскости в основной платой то придется еще дополнительно внести коррективы на положение датчика.
Skype - wmaster_s E-Mail - WorldMasters@gmail.com
Работаем по 3 критериям - быстро, качественно, недорого. Заказчик выбирает любые два.
WorldMaster вне форума Ответить с цитированием
Ответ


Купить рекламу на форуме - 42 тыс руб за месяц

Опции темы Поиск в этой теме
Поиск в этой теме:

Расширенный поиск


Похожие темы
Тема Автор Раздел Ответов Последнее сообщение
методы определения наклона изображений НуженБубен Помощь студентам 0 06.02.2017 22:20
Определить угол положения часовой стрелки - проверьте меня denzel_ua Общие вопросы C/C++ 1 30.10.2012 16:32
delphi. Подпрограммы. Определить угол А между векторами Х и У prettykatty Помощь студентам 11 28.06.2010 09:58
найти угол наклона tatee Помощь студентам 0 21.12.2009 01:29
Найти величину и угол наклона полного ускорения точки tatee Помощь студентам 0 07.12.2009 18:08