ロボット工作研究室Wiki

ロボット工作研究室の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 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);

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);
}

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);//次のコマンドに
}
```

×

この広告は60日間更新がないwikiに表示されております。