|
#include <dspic.h>
|
|
|
|
#include "setup.h"
|
|
#include "a6.h"
|
|
#include "asserv.h"
|
|
|
|
__CONFIG(FBS, BWRPU & BSSNB & NORBS);
|
|
__CONFIG(FSS, SWRPU & SSSNS & NORSS);
|
|
__CONFIG(FGS, GCPU & GSSU);
|
|
__CONFIG(FOSCSEL, FRCPLL & IESOEN);
|
|
__CONFIG(FOSC, FCKSMDIS & OSC2DIO & IOL1WAY);
|
|
__CONFIG(FWDT, WDTDIS & WINEN & WDTPRE128 & WDTPS32768);
|
|
__CONFIG(FPOR, PWMPORT & PWRTDIS);
|
|
__CONFIG(FICD, DEBUGEN & JTAGDIS & ICS1);
|
|
|
|
extern unsigned char arret;
|
|
extern unsigned char prog;
|
|
|
|
unsigned short i=0;
|
|
//short tab1[400], tab2[400], tab3[400], tab4[400];
|
|
//unsigned short id=0;
|
|
|
|
void main()
|
|
{
|
|
A6_packet a6;
|
|
unsigned char ch;
|
|
unsigned char data[4];
|
|
unsigned short val;
|
|
short p;
|
|
short vit;
|
|
|
|
setup_clock();
|
|
setup_io();
|
|
setup_A6();
|
|
setup_channel();
|
|
setup_asserv();
|
|
|
|
for(p=0;p<1000;p++);
|
|
|
|
#ifdef ASSERV_POS
|
|
while(1)
|
|
{
|
|
if(arret==1 && prog==1)
|
|
{
|
|
channel_read(INIT_POS, data);
|
|
setInitPos(*(unsigned short*)(&data[0]), *(unsigned short*)(&data[2]));
|
|
|
|
channel_read(INIT_THETA, data);
|
|
setInitTheta(*(unsigned short*)(&data[0]));
|
|
}
|
|
|
|
if(isChannelNew(DEST))
|
|
{
|
|
channel_read(DEST, data);
|
|
setDest(*(unsigned short*)(&data[0]), *(unsigned short*)(&data[2]));
|
|
}
|
|
if(isChannelNew(THETA_DEST))
|
|
{
|
|
channel_read(THETA_DEST, data);
|
|
setThetaDest(*(unsigned short*)(&data[0]));
|
|
}
|
|
if(isChannelNew(MODE_ASSERV))
|
|
{
|
|
channel_read(MODE_ASSERV, data);
|
|
setMode(data[0]);
|
|
}
|
|
if(isChannelNew(VIT))
|
|
{
|
|
channel_read(VIT, data);
|
|
vit=*(unsigned short*)(&data[0]);
|
|
if(vit<0)
|
|
{
|
|
vit=-vit;
|
|
setWay(Back);
|
|
}
|
|
else
|
|
{
|
|
setWay(Forward);
|
|
}
|
|
setVitesse(vit);
|
|
}
|
|
|
|
asserv();
|
|
|
|
data[0] = getState();
|
|
data[1] = 0;
|
|
*(unsigned short*)(&data[2]) = 0;
|
|
channel_set(ASSERV_STATE, data);
|
|
|
|
*(unsigned short*)(&data[0]) = getX();
|
|
*(unsigned short*)(&data[2]) = getY();
|
|
channel_set(POS, data);
|
|
|
|
*(unsigned short*)(&data[0]) = getTheta();
|
|
*(unsigned short*)(&data[2]) = 0;
|
|
channel_set(THETA, data);
|
|
|
|
*(unsigned short*)(&data[0]) = getDistance();
|
|
*(unsigned short*)(&data[2]) = 0;
|
|
channel_set(DIST, data);
|
|
}
|
|
#elif ASSERV_DEG
|
|
while(1)
|
|
{
|
|
if(arret==1 && prog==1)
|
|
{
|
|
|
|
}
|
|
if(isChannelNew(TOP_DEST))
|
|
{
|
|
channel_read(TOP_DEST, data);
|
|
setTopDest(*(unsigned short*)(&data[0]), *(unsigned short*)(&data[2]));
|
|
}
|
|
|
|
if(isChannelNew(POWER))
|
|
{
|
|
channel_read(POWER, data);
|
|
setPower(*(short*)(&data[0]));
|
|
}
|
|
|
|
asserv_deg();
|
|
|
|
*(unsigned short*)(&data[0]) = getTop();
|
|
*(unsigned short*)(&data[2]) = 0;
|
|
channel_set(TOP, data);
|
|
}
|
|
#else
|
|
#error "You have to define any asserv mode in configuration project"
|
|
#endif
|
|
}
|