МОДЕЛИРОВАНИЕ РОБОТОВ В iMATLAB

При моделировании динамики роботов соответствующие уравнения проще описывать на приспособленном к таким задачам языке iMatLab. Совместно с программированием микроконтроллера на AVR GCC. Интересна их связка. Совмещенное моделирование удобно, поскольку в нем происутствует не только микропроцессор с его особенностями, но также учитывается и среда, в которой действует робот.

ПРОГРАММНОЕ УПРАВЛЕНИЕ СКОРОСТЬЮ

Рассмотрим простой учебный пример, когда микроконтроллер управляет двигателем (скоростью V) автомобиля, динамика которого моделируется в iMatLab.


#define F_CPU 8000000UL // 8 MHz 
#include <avr/io.h> 
#include <util/delay.h>

int main(void){
// выключаем показ микроконтроллера
clear(); int T=0; // дискретное время
DDRD = 0xff; 
while(T<30){
// логика управления (старт-стоп-старт)
switch (true) {
case T<10: PORTD = 0xff; break; 
case T>20: PORTD = 0xff; break;
default: PORTD = 0x00; 
}
// управляем с второй ножки МК двигателем,
control(PORTD&1<<PD0); T++; 
}//while
}

%math, if tick=0, Robot=car(7), end,

% iMATLAB-подпрограмма управления скоростью V робота,

function: control(V), moveX(Robot 3*V), show(Robot), end,

function: car(L),
var C, C=iniFG(0 '#00aaaa' -L 0), 
addFG(C 'T' 'T' '#222222' [-L-2 -L+2 -L+2 -L-2] [-L-2 -L-2 L+2 L+2]),
addFG(C 'T' 'T' '#222222' [L-2 L+2 L+2 L-2] [-L-2 -L-2 L+2 L+2]), 
addFG(C 'T' 'T' '#ff9955' [-L-4 -L-4 L+5 L+5] [-L L L -L]),
addFG(C 'T' 'T' '#ffbb55' [-L -L L L] [-L L L -L]),
moveX(C 220), moveY(C 150), 
return C, 
end,

РОБОТ С ОПТИЧЕСКИМ СЕНСОРОМ

Нарисуем в iMatLab дорожку W с координатами Z, снабдим робот-тележку сенсором положения, оптодатчиком, прицепленным под задней осью (под карданом). Switch-контроллер, реализующий скользящий режим движения на краю дорожки, представлен следующим алгоритмом.


#define F_CPU 8000000UL // 8 MHz 
#include <avr/io.h> 
#include <util/delay.h>

int main(void){
// выключаем показ микропроцессора,
clear(), int T=0; // дискретное время
DDRD = 0xff; // выход
DDRC = 0x00; // вход
while(T<300){
// логика управления,
PORTC=sensor(Z); 
switch (PORTC&1<<PINC0) {
case 0: PORTD = 0x00; break; 
case 1: PORTD = 0xff; break;
}
// управляем с второй ножки МК двигателем,
control(PORTD&1<<PD0); T++; 
}//while
}

%math, 
if tick=0, 
call Z=street(0 140 435 160), W=way(Z),
X=[0 0 0], H=0.1, V=50, dt=0.2, C=car(10), end, 

% ТУЛБОКС РОБОТА,

function: Runge(X dt),
% SOLVE dX/dt=Fun(X),
var f a b X,  a={X},
f=Fun(X), b={f*dt}, X={{a+b/2}}, 
f=Fun(X), f={f*dt}, X={{a+f/2}}, b={{b+2*f}},  
f=Fun(X), f={f*dt}, X={a+f}, b={{b+2*f}},
f=Fun(X), f={f*dt}, X={{a+(b+f)/6}},
return X, 
end,

function: Fun(X), 
% динамика робота,
var f B, f=X[2], if f>PI, f=f-2*PI, end, if f<-PI, f=f+2*PI, end,
X[2]=f, B=[V*cos(f) V*sin(f) H*V*tan(s)], X={B*dt}, return X,
end,

function: street(X1 Y1 X2 Y2),
% ПРЯМОУГОЛЬНАЯ ДОРОЖКА,
var X Y Z, X=zero(1), Y=zero(1), 
X[0]=X1, Y[0]=Y1, X[1]=X1, Y[1]=Y2, 
X[2]=X2, Y[2]=Y2, X[3]=X2, Y[3]=Y1,
Z=[X Y], return Z, 
end, 

function: way(Z),
% внешний вид дорожки,
var x1 x2 x3 y1 y2 y3 y4 W, W=iniFG(0 '#222255' 0 0), 
x1=Z[0][0], y1=Z[1][0], x2=Z[0][1], y2=Z[1][1],
x3=Z[0][2], y3=Z[1][2], x4=Z[0][3], y4=Z[1][3],
addFG(W 'F' 'F' '#aaaaff' [x1 x2 x3 x4] [y1 y2 y3 y4]),
return W, 
end, 

% iMATLAB-подпрограмма управления поворотом робота,

function: control(PD), 
if X[0]>400, T=0, X=[0 0 0], else,
moveXat(C 10+X[0]), 
moveYat(C 150+X[1]), turnat(C X[2]), show(W C), 
% SWITCH-КОНТРОЛЛЕР,
switch PD
case 0, s=0.1, case 1, s=-0.1, % ПОВОРОТ РУЛЯ,
end,
call X=Runge(X dt), end, 
end,

function: sensor(Z),
% датчик нахождения на дорожке 1/0 (в кардане),
var c1 c2 X Y pX pY n b, 
pX=C[0][1], pY=C[1][1], X=Z[0], Y=Z[1], n=size(X), 
if X.length=n+1, X[n+1]=X[0], Y[n+1]=Y[0], end, b=0, 
for var i=0:n, 
if pY<=Y[i], c1=0, else, c1=1, end, 
if pY>Y[i+1], c2=0, else, c2=1, end, 
if c1=c2, c1=pX-X[i], c2=(pY-Y[i])*(X[i+1]-X[i])/(Y[i+1]-Y[i]), 
if c1>=c2, else, if b=1, b=0, else, b=1, end, 
end, end, end,
return b,
end,

function: car(L),
var C, C=iniFG(0 '#00aaaa' -L 0), 
addFG(C 'T' 'T' '#222222' [-L-2 -L+2 -L+2 -L-2] [-L-2 -L-2 L+2 L+2]),
addFG(C 'T' 'T' '#222222' [L-2 L+2 L+2 L-2] [-L-2 -L-2 L+2 L+2]), 
addFG(C 'T' 'T' '#ff9955' [-L-4 -L-4 L+5 L+5] [-L L L -L]),
addFG(C 'T' 'T' '#ffbb55' [-L -L L L] [-L L L -L]),
moveX(C 220), moveY(C 150), 
return C, 
end,



Rambler's Top100