ロボット工作研究室Wiki

utility.h

#ifndef __UTILITY
#define __UTILITY

float d_cos(float x);

int itoa(char *a, int x);
int itoaw(char *a, int x, unsigned int width);
int itoh(char *a, unsigned int x);
int ftoa(char *a, float x, int precision);
float atof(char *a);
int atoi(char *a);

float angle(float value);  // 0<t<360
float angle2(float value); // -180<t<180


#define PI         3.14159265359f
#define DEG_TO_RAD (PI / 180)
//#define SQR2       1.41421356
//#define RECSQR2    0.70710678 //逆数

#define square(x) ((x) * (x))

int ipower(int value, int n); //n乗
int isqrt(int a);             //平方根
int sgn(int value);           //符号
int iabs(int value);          //絶対値

float fAbs(float value);

int pmod(int val1, unsigned int val2);

int limit_zero(int value);
int limit_max(int value, int max);
int limit_min(int value, int min);
int limit_scope(int value, int min, int max);
float flimit_scope(float value, float min, float max);


#endif /*__UTILITY */

utility.c

#include "utility.h"



#define TABLESIZE 128 //2の乗数であること
#define SUBBIT    8
#define SUBINDEX  (1 << SUBBIT)
#define I_PI      (TABLESIZE * SUBINDEX * 2)
#define I_HPI     (TABLESIZE * SUBINDEX)


//int(sin(index / (pi / 2) * 128) * 65535 + 0.5)
const unsigned short sin_table[] = {
    0,   804,  1608,  2412,  3216,  4019,  4821,  5623, 
 6424,  7223,  8022,  8820,  9616, 10411, 11204, 11996, 
12785, 13573, 14359, 15142, 15924, 16703, 17479, 18253, 
19024, 19792, 20557, 21319, 22078, 22834, 23586, 24334, 
25079, 25820, 26557, 27291, 28020, 28745, 29465, 30181, 
30893, 31600, 32302, 32999, 33692, 34379, 35061, 35738, 
36409, 37075, 37736, 38390, 39039, 39682, 40319, 40950, 
41575, 42194, 42806, 43411, 44011, 44603, 45189, 45768, 
46340, 46905, 47464, 48014, 48558, 49095, 49624, 50145, 
50659, 51166, 51664, 52155, 52638, 53113, 53580, 54039, 
54490, 54933, 55367, 55794, 56211, 56620, 57021, 57413, 
57797, 58171, 58537, 58895, 59243, 59582, 59913, 60234, 
60546, 60850, 61144, 61429, 61704, 61970, 62227, 62475, 
62713, 62942, 63161, 63371, 63571, 63762, 63943, 64114, 
64276, 64428, 64570, 64703, 64826, 64939, 65042, 65136, 
65219, 65293, 65357, 65412, 65456, 65491, 65515, 65530, 
65535, 0
};

float d_cos(float x){
    long ix, subix, sign, tval;
    
    ix = (int)(x * (I_PI / 180.f));   //単位変換
    ix += I_HPI;
    sign = ix & I_PI;              //第3,第4象限である
    ix &= (I_PI - 1);              //第1,第2象限に限定
    if(ix > I_HPI) ix = I_PI - ix; //第1象限に限定
    
    subix = ix & (SUBINDEX - 1);   //線形補完に用いるサブインデックス
    ix >>= SUBBIT;                 //テーブル番号に変換
    
    //線形補完
    tval = ((long)sin_table[ix]   * (SUBINDEX - subix)
          + (long)sin_table[ix+1] * subix);
    
    return (sign ? -tval : tval) / (SUBINDEX * 65535.f);
}




//角度を0~360degの範囲に収める
float angle(float value){
    
    while(1){
        if(value < 0){
            value += 360;
        }else if(value >= 360){
            value -= 360;
        }else{
            break;
        }
    }
    
    return(value);
}

//角度を-180~180degの範囲に収める
float angle2(float value){
    
    while(1){
        if(value < -180){
            value += 360;
        }else if(value >= 180){
            value -= 360;
        }else{
            break;
        }
    }
    
    return(value);
}

//整数のn乗
int ipower(int value, int n){
    int ret = 1;
    
    for(;n>0;n--){
        ret *= value;
    }
    
    return(ret);
}

//整数の平方根近似
/*int isqrt(int a)
{
    int i;
    
    //a /= 2;
    a >>= 1;
    for (i = 1; a > 0; i++) a -= i;
    
    return i - 1;
}*/

int isqrt(int f)
{
    int s = f, t;
    
    if (f == 0) return 0;
    do
    {
        t = s;
        s = (t + f / t) >> 1;
    } while (s < t);

    return t;
}

//整数の符号を返す
int sgn(int value){
    if(value > 0){
        return 1;
    }else if(value < 0){
        return -1;
    }
    return 0;
}

//整数の絶対値
int iabs(int value){
    return (value < 0 ? -value : value);
}

//実数の絶対値
float fAbs(float value){
    return (value < 0 ? -value : value);
}

//常に正の余りを返す
int pmod(int val1, unsigned int val2){
    
    val1 = val1 % (int)val2;
    if(val1 < 0) val1 += val2;
    
    return val1;
}

//0以下の値を0に切り上げ
int limit_zero(int value){
    return ((value < 0) ? 0 : value);
}

//max以上の値をmaxに切り下げ
int limit_max(int value, int max){
    return ((value > max) ? max : value);
}

//min以下の値をminに切り上げ
int limit_min(int value, int min){
    return ((value < min) ? min : value);
}

//範囲内に切り上げまたは切り下げ
int limit_scope(int value, int min, int max){
    if(value < min){
        value = min;
    }else if(value > max){
        value = max;
    }
    return(value);
}

//実数を範囲内に切り上げまたは切り下げ
float flimit_scope(float value, float min, float max){
    if(value < min){
        value = min;
    }else if(value > max){
        value = max;
    }
    return(value);
}





int itoa(char *a, int x){
    return itoaw(a, x, 0);
}

int itoaw(char *a, int x, unsigned int width){
    int place = 1000000000;
    int firstnum = 0;
    int k, n = 0;
    
    if(x < 0){
        a[0] = '-';
        n++;
        x = -x;
    }
    
    while(place){
        k = x / place;
        
        if(k || width >= 10) firstnum = 1;
        
        if(firstnum || place == 1){
            a[n] = '0' + k;
            n++;
            
            x -= k * place;
        }
        
        place /= 10;
        width++;
        
    }

    a[n] = 0;

    return(n);
}

int itoh(char *a, unsigned int x){
    unsigned int place = 0x80000000;
    int firstnum = 0;
    int k, n = 0;
    
    while(place){
        k = x / place;
        if(k) firstnum = 1;
        
        if(firstnum || place == 1){
            a[n] = (k < 10) ? ('0' + k) : (('A' - 10) + k);
            n++;
            
            x -= k * place;
        }
        
        place >>= 4;
        
    }
    
    a[n] = 0;

    return(n);
}

int ftoa(char *a, float x, int precision){
    int n = 0;
    
    if(x < 0){
        a[0] = '-';
        n++;
        x = -x;
    }
    
    n += itoa(a + n, (int)x);
    x -= (int)x;
    
    if(precision > 0){
        a[n] = '.';
        n++;
        
        n += itoaw(a + n, (int)(x * ipower(10, precision)), precision);
        
    }
    
    return(n);
}

float atof(char *a){
    float ret = 0;
    int pointed = 0;
    char psgn = 0;
    
    if(*a == '-'){
        psgn = 1;
        a++;
    }
    
    while(*a != 0){
        if('0' <= *a && *a <= '9'){
            if(pointed){
                ret += (float)(*a - '0') / (float)pointed;
                pointed *= 10;
            }else{
                ret *= 10;
                ret += (float)(*a - '0');
            }
        }else if(*a == '.'){
            pointed = 10;
        }else{
            break;
        }
        
        a++;
    }
    
    return psgn ? -ret : ret;
}

int atoi(char *a){
    int ret = 0;
    char psgn = 0;
    
    if(*a == '-'){
        psgn = 1;
        a++;
    }
    
    while(*a != 0){
        if('0' <= *a && *a <= '9'){
            ret *= 10;
            ret += (int)(*a - '0');
        
        }else{
            break;
        }
        
        a++;
    }
    
    return psgn ? -ret : ret;
}

usart.h

#include <stdarg.h>

void usart_init();
void usart_putc(const char c);
void usart_putc_nowait(const char c);
char usart_getc();
void usart_printstr(const char* str);
void usart_inputstr(char* str);
void usart_scanstr(char* str);
void usart_printint(int val);
void usart_printfloat(float value, int decimal);
int  usart_received();

void usart_printf(const char *fmt, ...);

void roboterm_clear();
void roboterm_settarget(int number);
void roboterm_setlabel(int number, const char* str);
void roboterm_setvalue(int number, const char* str);
void roboterm_setvalueint(int number, int value);
void roboterm_setvaluef(int number, float value, int decimal);

void roboterm_setcategory(const char *a);
float roboterm_fgetparam(const char *name);
int roboterm_igetparam(const char *name);
void roboterm_sgetparam(const char *name, char *str);

void roboterm_selecttab(int index);

usart.c

#include "stm32f10x.h"
#include "usart.h"
#include "utility.h"


#define ROBOTERM

//USART Control Functions ///////////////////////////////////////////////////////

#define CHAR_NULL 0x00
#define CHAR_CR   0x0d
#define CHAR_LF   0x0a

//改行コードはWindows仕様に
#define PUT_NEWLINE  usart_putc(CHAR_CR); usart_putc(CHAR_LF)

void usart_init(){
    
    USART_InitTypeDef USART_InitStructure;
    
    USART_InitStructure.USART_BaudRate = 250000;
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
    USART_InitStructure.USART_StopBits = USART_StopBits_1;
    USART_InitStructure.USART_Parity = USART_Parity_No ;
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
    
    USART_Init(USART1, &USART_InitStructure);
    /* Enable the USART1 */
    USART_Cmd(USART1, ENABLE);
    
}

void usart_putc(const char c){
    while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);
    USART_SendData(USART1, c);
    while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);
}

void usart_putc_nowait(const char c){
    while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);
    USART_SendData(USART1, c);
}

char usart_getc(){
    while(USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET);
    return USART_ReceiveData(USART1);USART_ReceiveData(USART1);
}


int usart_received(){
    if(USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET){
        return 0;
    }
    
    return 1;
}


//文字列を送信する
void usart_printstr(const char* str){

    while(*str != CHAR_NULL) {

        
        if(*str == '\n'){
            PUT_NEWLINE;
        }else{
            usart_putc(*str);
        }
        
        str++;
    }
}

//文字列を受け取り、リターンで終了
void usart_inputstr(char* str){
    char c;

    while(1){
        c = usart_getc();
        
        if(c == CHAR_CR){
            *str = CHAR_NULL;
            break;
        }
        
        *str = c;
        str++;
        
    }

}

//入力を逐次再送信し、リターンで終了
void usart_scanstr(char* str){
    char c;

    while(1){
        c = usart_getc();
        
        if(c == CHAR_CR){
            usart_putc(CHAR_CR);
            usart_putc(CHAR_LF);
            *str = CHAR_NULL;
            break;
        }
        
        usart_putc(c);
        *str = c;
        str++;
        
    }
}

#ifdef __UTILITY

void usart_printint(int value){
    char a[10];
    
    itoa(a, value);
    
    usart_printstr(a);
}

void usart_printfloat(float value, int decimal){
    char a[10];
    
    ftoa(a, value, decimal);
    
    usart_printstr(a);
}





//簡易版printf: %d, %sのみ対応
//フィールド幅などの指定は不可
void usart_printf(const char *fmt, ...){
    va_list ap;
    
    va_start(ap, fmt);
    
    while(*fmt != CHAR_NULL) {
        
        if(*fmt == '%'){
            fmt++;
            if(*fmt == 'd')      usart_printint(va_arg(ap, int));
            else if(*fmt == 's') usart_printstr(va_arg(ap, const char *));
            else if(*fmt == '%') usart_putc('%');
        }else if(*fmt == '\n'){
            PUT_NEWLINE;
        }else{
            usart_putc(*fmt);
        }
        
        fmt++;
    }
    
    va_end(ap);
    
    
}

#endif



#ifdef ROBOTERM

void roboterm_clear(){
    usart_printstr("@CL:");
}

void roboterm_settarget(int number){
    usart_printstr("@V");
    usart_printint(number);
    usart_printstr(":");
}

void roboterm_setlabel(int number, const char* str){
    usart_printstr("@L");
    usart_printint(number);
    usart_printstr(":");
    
    usart_printstr(str);
}

void roboterm_setvalue(int number, const char* str){
    usart_printstr("@V");
    usart_printint(number);
    usart_printstr(":");
    
    usart_printstr(str);
}

void roboterm_setvalueint(int number, int value){
    usart_printstr("@V");
    usart_printint(number);
    usart_printstr(":");
    
    usart_printint(value);
}

void roboterm_setvaluef(int number, float value, int decimal){
    usart_printstr("@V");
    usart_printint(number);
    usart_printstr(":");
    
    usart_printfloat(value, decimal);
}

void roboterm_setcategory(const char *a){
    usart_printstr("@PC");
    usart_printstr(a);
    usart_printstr(":");
}

float roboterm_fgetparam(const char *name){
    char a[10];
    
    usart_printstr("@PG");
    usart_printstr(name);
    usart_printstr(":");
    
    usart_inputstr(a);
    
    return(atof(a));
}

int roboterm_igetparam(const char *name){
    char a[10];
    
    usart_printstr("@PG");
    usart_printstr(name);
    usart_printstr(":");
    
    usart_inputstr(a);
    
    return(atoi(a));
}

void roboterm_sgetparam(const char *name, char *str){
    usart_printstr("@PG");
    usart_printstr(name);
    usart_printstr(":");
    
    usart_inputstr(str);
}

void roboterm_selecttab(int index){
    usart_printstr("@T");
    usart_printint(index);
    usart_printstr(":");
}

#endif

//減速に必要な距離の２乗
            float nl2 = square((square(im.speed) - square(target_speed)) / (2 * deccel));
            float dl = im.target_u - u;
            float dl2 = square(dl);
            float dlf2 = square(dl - stabilityLength); //目標までの距離の２乗
            
            
            if(dl <= stabilityLength){ //距離調整
                //1ステップで速度調整するために必要な加速度
                im.accel = (target_speed - im.speed) * dti;
                //加速度の制限
                im.accel = flimit_scope(im.accel, -deccel, deccel);
                
            }else if(dlf2 <= nl2 && im.speed > 120){ //速度調整(発進時は猶予を)
                
                if(dl2 < nl2){ //間に合わなさそうなときは加速度を増す
                    deccel += DA * dt;
                    if(deccel > maxaccel) deccel = maxaccel;
                }
                
                
                if(target_speed < im.speed){ 
                    im.accel = -deccel; 
                }else{
                    //加速度を上げる時は加速度上昇量を守る
                    im.accel += flimit_scope(deccel - im.accel, -DA * dt, DA * dt);
                }
                
                
                accelmode = 0; //一旦減速したら最加速はしない
                
            }else if(!accelmode){
                //残りの距離が余ったら、加速度を少し落とす
                deccel -= DA * dt;
                
                if(target_speed < im.speed) im.accel = -deccel; else im.accel = deccel;
                
            }else if(im.speed < maxspd
                 && !(dl < HALFBLOCK && im.speed >= target_speed)){ //目標まで半区画以下で目標速度以上の加速を禁止
                
                if(im.speed > ACCREST_SPEED){ // ある速度以上では加速度を制限する
                    if(im.accel > adaccel * 0.75f) im.accel -= DA * dt;
                }else{
                    if(im.accel < adaccel) im.accel += DA * dt;
                }
                
            }else{
                
                im.accel = 0;
            }
            
            im.speed += im.accel * dt;
            if(im.speed < SPEED_MINIMUM) im.speed = SPEED_MINIMUM;
            im.u = u + im.speed * dt;
            
            
            if(im.u >= im.target_u){
                im.u = im.target_u;
                im.accel = 0;
                
                Backyard.tcount = 0;
                
                Backyard.command_index++; Backyard_OperationCommand(0);//次のコマンドに
            }