分类： C/C++
2009-06-04 10:32:17
源程序如下，其中函数IsContourP用来判断某点是不是边界点。
BOOL Contour(HWND hWnd)
{
DWORD OffBits，BufSize;
LPBITMAPINFOHEADER lpImgData;
LPSTR lpPtr;
HLOCAL hTempImgData;
LPBITMAPINFOHEADER lpTempImgData;
LPSTR lpTempPtr;
HDC hDc;
HFILE hf;
LONG x,y;
POINT StartP,CurP;
BOOL found;
int i;
int direct[8][2]={{1,0},{1,-1},{0,-1},{-1,-1},{-1,0},
{-1,1},{0,1},{1,1}};
//我们处理的实际上是256级灰度图，不过只用到了0和255两种颜色。
if( NumColors!=256){
MessageBox(hWnd,"Must be a mono bitmap with grayscale palette!","Error Message",MB_OK|MB_ICONEXCLAMATION);
return FALSE;
}
//到位图数据的偏移值
OffBits=bf.bfOffBits-sizeof(BITMAPFILEHEADER);
//缓冲区大小
BufSize=bf.bfSize-sizeof(BITMAPFILEHEADER);
//为新图缓冲区分配内存
if((hTempImgData=LocalAlloc(LHND,BufSize))==NULL)
{
MessageBox(hWnd,"Error alloc memory!","Error Message",MB_OK|
MB_ICONEXCLAMATION);
return FALSE;
}
lpImgData=(LPBITMAPINFOHEADER)GlobalLock(hImgData);
lpTempImgData=(LPBITMAPINFOHEADER)LocalLock(hTempImgData);
//新图缓冲区初始化为255
memset(lpTempImgData,(BYTE)255,BufSize);
//拷贝头信息
memcpy(lpTempImgData,lpImgData,OffBits);
//找到标志置为假
found=FALSE;
for (y=0;y
for (x=0;x
}
if(found){ //如果找到了，才做处理
//从上面的循环退出时，x，y坐标都做了加1的操作。在这里把它们减1，得到起
//始点坐标StartP
StartP.x=x-1;
StartP.y=y-1;
lpTempPtr=(char *)lpTempImgData+(BufSize-
LineBytes-StartP.y*LineBytes)+StartP.x;
*lpTempPtr=(unsigned char)0; //起始点涂黑
//右邻点
CurP.x=StartP.x+1;
CurP.y=StartP.y;
lpPtr=(char *)lpImgData+(BufSize-LineBytes-CurP.y*LineBytes)+CurP.x;
if(*lpPtr!=0){ //若右邻点为白，则找右下邻点
CurP.x=StartP.x+1;
CurP.y=StartP.y+1;
lpPtr=(char *)lpImgData+(BufSize-LineBytes-CurP.y*LineBytes)+CurP.x;
if(*lpPtr!=0){ //若仍为白，则找下邻点
CurP.x=StartP.x;
CurP.y=StartP.y+1;
}
else{ //若仍为白，则找左下邻点
CurP.x=StartP.x-1;
CurP.y=StartP.y+1;
}
}
while (! ( (CurP.x==StartP.x) &&(CurP.y==StartP.y))){
//知道找到起始点，循环才结束
lpTempPtr=(char *)lpTempImgData+(BufSize-LineBytes
-CurP.y*LineBytes)+CurP.x;
*lpTempPtr=(unsigned char)0;
for(i=0;i<8;i++){
//按右，右上，上，左上，左，左下，下，右下的顺序找相邻点
//direct[i]中存放的是该方向x,y的偏移值
x=CurP.x+direct[i][0];
y=CurP.y+direct[i][1];
//lpPtr指向原图数据，lpTempPtr指向新图数据
lpTempPtr=(char *)lpTempImgData
+(BufSize-LineBytes-y*LineBytes)+x;
lpPtr=(char *)lpImgData+(BufSize-LineBytes-y*LineBytes)+x;
if( ( (*lpPtr==0)&&(*lpTempPtr!=0) ) ||
( (x==StartP.x) &&(y==StartP.y)))
//原图中为黑点，且新图中为白点（表示还没搜索过）时才处理
//另一种可能是找到了起始点
if(IsContourP(x,y,lpPtr)){ //若是个边界点
//记住当前点的位置
CurP.x=x;
CurP.y=y;
break;
}
}
}
}
if(hBitmap!=NULL)
DeleteObject(hBitmap);
hDc=GetDC(hWnd);
//创立一个新的位图
hBitmap=CreateDIBitmap(hDc,(LPBITMAPINFOHEADER)lpTempImgData,
(LONG)CBM_INIT,(LPSTR)lpTempImgData+sizeof(BITMAPINFOHEADER) +
NumColors*sizeof(RGBQUAD),(LPBITMAPINFO)lpTempImgData, DIB_RGB_COLORS);
hf=_lcreat("c:\\contour.bmp",0);
_lwrite(hf,(LPSTR)&bf,sizeof(BITMAPFILEHEADER));
_lwrite(hf,(LPSTR)lpTempImgData,BufSize);
_lclose(hf);
//释放内存和资源
ReleaseDC(hWnd,hDc);
LocalUnlock(hTempImgData);
LocalFree(hTempImgData);
GlobalUnlock(hImgData);
return TRUE;
}
//判断某点是不是边界点，参数x,y 为该点的坐标，lpPtr为指向原位图数据的指针
BOOL IsContourP(LONG x,LONG y, char *lpPtr)
{
int num,n,w,e,s;
n=(unsigned char)*(lpPtr+LineBytes); //上邻点
w=(unsigned char)*(lpPtr-1); //左邻点
e=(unsigned char)*(lpPtr+1); //右邻点
s=(unsigned char)*(lpPtr-LineBytes); //下邻点
num=n+w+e+s;
if(num==0) //全是黑点，说明是个内部点而不是边界点
return FALSE;
return TRUE;
}
java方面的：
it人资讯交流网
这个网站是我最近才发现的，虽然内容不多，但是提供的相关java工具挺齐全。还有就是里面提供了java教学视频录象的免费下载，好像一两周更换一段。个人觉得挺适合初学者的，尤其是那个classpath的设置，讲的很透彻，大家有空可以看一看。
java官方站点(英文)
要想了解最新的java动态，下载最新的java相关，比如j2se、j2ee、j2se的最新jdk版本就来这里吧。
java中文站
这个可能大家都知道，不用说了，他提供的java资源是最丰富的。注册论坛是免费的，还送积分，用积分可以下载软件和电子书等，如果积分用完了，就需要自己发表一些文章来赚新的积分。
中文java网站
跟上面站点类似的一个站，宗旨就是：为java爱好者服务。值得一看！
锋网
综合性的java网站，内含“下载中心”、“教程教学”等栏目。
java动力
网站的内容可以，但是最为出色的是它所运用的flash技术，我就不在这里多说了，大家去看看就知道了，一个字“酷”！！！
vc方面的：
vc知识库
这个网站就不用多说了，学习vc必去之地。网站专门提供了免费的ftp下载，好东东巨多！
vc之路
综合软件开发网站，以vc为主。“资源中心”有许多值得下载的东东。
visual c++/mfc开发指南
以讲述windows开发为主的站点，提供了最新的源代码，开发工具，开发资料，开发教程和对好的开发站点，开发工具，图书做介绍，同时为从事开发的朋友提供发布自己开发的软件，代码和工具场所。
c维视点
最近发现的vc好站，书籍、软件、代码下载一应具全！！！
游戏开发：
风云工作室
~cloudwu/2000/index.html
标点游戏制作
未来开发者
http://www.fdev.net/
综合的：
中国软件网
中国最大的开发者网络，他之所以著名就是因为他的论坛，大家有空可以去看看，能下到很多不错的东东，另外也是交流学习的好地方。
电子书籍的：
最后公布一个巨好的，狂多的电子书下载
还有巨好的
biplip移动开发网论坛
雷神的WINCE页面
eMbedded Programming
华人wince论坛
美斯比论坛
移动技术网社区
嵌入式研究网
eMbedded Visual C++论坛 CN Dev Forum
http://www.copathway.com/cndevforum/cndevforum.asp
新闻组
(EN)
新闻组
(EN)ICP=embedded&sLCID=US&newsgroup=microsoft.public.windowsce.embedded.vc(EN)
Pocket PC Developer Network
(EN)
(EN)
CE Programming
(EN)
(EN)
windows embedded tech bulletin
(EN)
开源网站介绍
开源网站，和编程技术网站不同的是提供和作者直接交流和源代码管理平台(subversion或cvs)
，最全的开源网站，支持所有语言，许多经典项目，如电驴，BT，python（语言解释器），7zip，phpMyAdmin，FileZilla，WinSCP等等都是在上面发展壮大的
，都是和apache有关并受apache基金支持的开源项目，如httpd，lucene，项目语言也很杂
，主要是C++语言项目，subversion是在它上面开源的，另外还有一些编程辅助工具的开源项目
此外还有一些特定语言的开源网站，如
，ruby开源项目
，vc++开源项目
itepub.com的电子书绝对经典
| 文章标题：VC++6.0实现视频数据实时获取的探讨 原 作 者：谭佐军 吴鹏飞 原 出 处：vczx.com 发 布 者：谭佐军 发布类型：原创 发布日期：2006-09-05 今日/总浏览：2/4776 |
摘要 在VC++6.0中利用VFW技术实现视频数据实时获取的过程中,针对回调函数的处理中所碰到的问题、图像采集中原始数据的获取以及捕获窗口的建立的问题进行了详细的讨论，并给出了具体的解决办法。
(1) 不使用成员函数，直接使用普通C函数，为了实现在C函数中可以访问类的成员变量，可以使用友元操作符(friend)，在C++中将该C函数说明为类的友元即可。这种处理机制与普通的C编程中使用回调函数一样。 (2) 使用静态成员函数，静态成员函数不使用this指针作为隐含参数，这样就可以作为回调函数了。静态成员函数具有两大特点：其一，可以在没有类实例的情况下使用；其二，只能访问静态成员变量和静态成员函数，不能访问非静态成员变量和非静态成员函数。由于在C++中使用类成员函数作为回调函数的目的就是为了访问所有的成员变量和成员函数，如果作不到这一点将不具有实际意义。解决的办法也很简单，就是使用一个静态类指针作为类成员，通过在类创建时初始化该静态指针，如pThis=this，然后在回调函数中通过该静态指针就可以访问所有成员变量和成员函数了。这种处理办法适用于只有一个类实例的情况，因为多个类实例将共享静态类成员和静态成员函数，这就导致静态指针指向最后创建的类实例。为了避免这种情况，可以使用回调函数的一个参数来传递this指针，从而实现数据成员共享。这种方法稍稍麻烦，这里就不再赘述。 因此，可以对回调函数作如下定义:
视频数据的采集是整个应用的关键，根据应用的不同可以将视频帧采集到的文件或采集的缓存直接加以处理。利用VFW获取实时视频数据通常可以运用视频处理的回调机制(call-backmechanism) 获得实时数据缓冲区的首址和长度并对图像数据进行实时处理，但是在这个过程中图像处理程序不能太长，否则视频显示不流畅。另外，在实际采集过程中，有些图像采集卡程序对通过回调机制所获取的数据进行了压缩，比如DC10，而在图像测量系统高精度的要求，压缩后的图像数据直接影响以后的图像处理工作。通过采用以下两种典型技术可以实现将没有压缩的视频帧的图像数据的采集。 (1) 通过研究发现，尽管图像采集卡驱动程序对通过回调机制所获取的数据进行了压缩，但是利用VFW中的capEditCopy( )宏函数将帧图像缓冲区中的图像数据拷贝到剪贴板上时，并没有压缩图像数据，因此可以不采用回调机制而直接利用capGrabFrameNoStop()捕获一帧图像，然后将数据拷贝拷贝到剪贴板上，再通过DIB（Device Independent Bitmap）操作获取内存中图像数据首地址，进行后续的图像数据处理。具体的代码片段如下：
(2) 利用capFileSaveDIB将缓冲区中的图像数据转化为DIB位图直接保存为文件，需要处理时，再读取位图中的图像数据到内存进行后续的处理。这种方式因为有一个文件存储和读取的延迟，对于实时的图像处理来说，响应速度比前者要稍慢，经过多次实验证明，只要图像处理算法的计算量不是很大，仍然可以保证比较好的实时性。
结束语 | |||||
//B = 1.164(Y - 16) + 2.018(U - 128)
//G = 1.164(Y - 16) - 0.813(V - 128) - 0.391(U - 128)
//R = 1.164(Y - 16) + 1.596(V - 128)
BYTE *orgRGBbuff = RGBbuff;
for(DWORD count="0";count
//Y0 U0 Y1 V0
float Y0 = *YUY2buff;
float U = *(++YUY2buff);
float Y1 = *(++YUY2buff);
float V = *(++YUY2buff);
++YUY2buff;
*(RGBbuff) = (BYTE)(Y0 + (1.370705 * (V-128)));
*(++RGBbuff) = (BYTE)(Y0 - (0.698001 * (V-128)) - (0.337633 * (U-128)));
*(++RGBbuff) = (BYTE)(Y0 + (1.732446 * (U-128)));
*(++RGBbuff) = (BYTE)(Y1 + (1.370705 * (V-128)));
*(++RGBbuff) = (BYTE)(Y1 - (0.698001 * (V-128)) - (0.337633 * (U-128)));
*(++RGBbuff) = (BYTE)(Y1 + (1.732446 * (U-128)));
++RGBbuff;
}
}
inline void YUY2_RGB2(BYTE *YUY2buff,BYTE *RGBbuff,DWORD dwSize)
{
//R = Y + 1.14V
//G = Y - 0.39U - 0.58V
//B = Y + 2.03U
// R = Y + 1.403V'
//G = Y - 0.344U' - 0.714V'
//B = Y + 1.770U'
BYTE *orgRGBbuff = RGBbuff;
for(DWORD count="0";count
//Y0 U0 Y1 V0
float Y0 = *YUY2buff;
float U = *(++YUY2buff);
float Y1 = *(++YUY2buff);
float V = *(++YUY2buff);
++YUY2buff;
float R,G,B;
R = (Y0 + 1.14f*V);
G=(Y0 - 0.39f*U-0.58f*V);
B=(Y0 +2.03f*U);
if(R<0) R =0;
if(R>255) R="255";
if(G<0) G =0;
if(G>255) G="255";
if(B<0) B =0;
if(B>255) B="255";
*(RGBbuff) = (BYTE)B;
*(++RGBbuff) = (BYTE)G;
*(++RGBbuff) = (BYTE)R;
R = (Y1 + 1.14f*V);
G=(Y1 - 0.39f*U-0.58f*V);
B=(Y1 +2.03f*U) ;
if(R<0) R =0;
if(R>255) R="255";
if(G<0) G =0;
if(G>255) G="255";
if(B<0) B =0;
if(B>255) B="255";
*(++RGBbuff) = (BYTE)B;
*(++RGBbuff) = (BYTE)G;
*(++RGBbuff) = (BYTE)R;
++RGBbuff;
}
}
inline void YUY2_RGB4(BYTE *YUY2buff,BYTE *RGBbuff,DWORD dwSize)
{
// R = Y + 1.403V'
//G = Y - 0.344U' - 0.714V'
//B = Y + 1.770U'
BYTE *orgRGBbuff = RGBbuff;
for(DWORD count="0";count
//Y0 U0 Y1 V0
float Y0 = *YUY2buff;
float U = *(++YUY2buff);
float Y1 = *(++YUY2buff);
float V = *(++YUY2buff);
++YUY2buff;
float R,G,B;
R = (Y0 + 1.403f*V);
G=(Y0 - 0.344f*U-0.714f*V);
B=(Y0 +1.77f*U);
if(R<0) R =0;
if(R>255) R="255";
if(G<0) G =0;
if(G>255) G="255";
if(B<0) B =0;
if(B>255) B="255";
*(RGBbuff) = (BYTE)R;
*(++RGBbuff) = (BYTE)G;
*(++RGBbuff) = (BYTE)B;
R = (Y1 + 1.403f*V);
G=(Y1 - 0.344f*U-0.714f*V);
B=(Y1 +1.77f*U);
if(R<0) R =0;
if(R>255) R="255";
if(G<0) G =0;
if(G>255) G="255";
if(B<0) B =0;
if(B>255) B="255";
*(++RGBbuff) = (BYTE)R;
*(++RGBbuff) = (BYTE)G;
*(++RGBbuff) = (BYTE)B;
++RGBbuff;
}
}
inline void YUY2_RGB3(BYTE *YUY2buff,BYTE *RGBbuff,DWORD dwSize)
{
//B = 1.164(Y - 16) + 2.018(U - 128)
//G = 1.164(Y - 16) - 0.813(V - 128) - 0.391(U - 128)
//R = 1.164(Y - 16) + 1.596(V - 128)
BYTE *orgRGBbuff = RGBbuff;
for(DWORD count="0";count
//Y0 U0 Y1 V0
float Y0 = *YUY2buff;
float U = *(++YUY2buff);
float Y1 = *(++YUY2buff);
float V = *(++YUY2buff);
++YUY2buff;
*(RGBbuff) = (BYTE)(1.164f*(Y0 - 16)+ 2.018f*(U - 128));
*(++RGBbuff) = (BYTE)(1.164f*(Y0 - 16) - 0.813f*(V - 128) - 0.391f*(U - 128));
*(++RGBbuff) = (BYTE)(1.164f*(Y0 - 16) + 1.596f*(V - 128));
*(++RGBbuff) = (BYTE)(1.164f*(Y0 - 16)+ 2.018f*(U - 128));
*(++RGBbuff) = (BYTE)(1.164f*(Y0 - 16) - 0.813f*(V - 128) - 0.391f*(U - 128));
*(++RGBbuff) = (BYTE)(1.164f*(Y0 - 16) + 1.596f*(V - 128));
++RGBbuff;
}
}
//
void VC4000Graber::YUY2_RGB(BYTE *YUY2buff,BYTE *RGBbuff,DWORD dwSize)
{
//
//C = Y - 16
//D = U - 128
//E = V - 128
//R = clip(( 298 * C + 409 * E + 128) >> 8)
//G = clip(( 298 * C - 100 * D - 208 * E + 128) >> 8)
//B = clip(( 298 * C + 516 * D + 128) >> 8)
BYTE *orgRGBbuff = RGBbuff;
for(DWORD count="0";count
//Y0 U0 Y1 V0
BYTE Y0 = *YUY2buff;
BYTE U = *(++YUY2buff);
BYTE Y1 = *(++YUY2buff);
BYTE V = *(++YUY2buff);
++YUY2buff;
LONG Y,C,D,E;
BYTE R,G,B;
Y = Y0;
C = Y-16;
D = U-128;
E = V-128;
R = clip255(( 298 * C + 409 * E + 128) >> 8);
G = clip255(( 298 * C - 100 * D - 208 * E + 128) >> 8);
B = clip255(( 298 * C + 516 * D + 128) >> 8);
*(RGBbuff) = B;
*(++RGBbuff) = G;
*(++RGBbuff) = R;
Y = Y1;
C = Y-16;
D = U-128;
E = V-128;
R = clip255(( 298 * C + 409 * E + 128) >> 8);
G = clip255(( 298 * C - 100 * D - 208 * E + 128) >> 8);
B = clip255(( 298 * C + 516 * D + 128) >> 8);
*(++RGBbuff) = B;
*(++RGBbuff) = G;
*(++RGBbuff) = R;
++RGBbuff;
}
}
BYTE VC4000Graber::clip255(LONG v)
{
if(v<0) v="0";
else if(v>255) v="255";
return (BYTE)v;
}
#ifndef __PID__H
#define __PID__H
#pragma code
#ifdef DEBUG_PID
#define SEND_PID_STRING(STRING) send_string((STRING))
#define SEND_PID_UCHAR_HEXA(CHR) send_unsigned_char_as_hexa((CHR))
#define SEND_PID_INT(INT) send_int_as_hexa((INT))
#define SEND_PID_LONG_HEXA(LNG) send_long_as_hexa((LNG))
#define SEND_PID_BYTE(CHR) send_byte((CHR))
static char string_state[]="State: ";
static char string_control[]="CTRL_Term: ";
static char string_error_term[]="Err Term: ";
static unsigned short counter = 0;
static char new_line[]="\r\n";
static char string_int_term[]="Int Term: ";
static char string_pos_dif[]="Pos Dif: ";
static char string_des_vel[]="Des. Veloc:"; ? static char string_des_ask[]="Ask. Veloc:";
static char string_ask_acc[]="Ask. Accel:";
static char string_des_pos[]="Des. Pos:";
static char string_int_term_of[]="Int.Term. OverFlow!";
static char string_control_overflow[]="Cont.Term Overflow!";
static char string_control_term[]="Ctrl. Term:";
static char string_pwm[]="PWM: ";
#else
#define SEND_PID_STRING(STRING) 1
#define SEND_PID_UCHAR_HEXA(CHR) 1
#define SEND_PID_INT(INT) 1
#define SEND_PID_LONG_HEXA(LNG) 1
#define SEND_PID_BYTE(CHR) 1
#endif
#pragma udata
unsigned char traj_finished;
// Buffer to copy the current mesured position
signed long mesured_position_buf[2];
#pragma code
/********************************************************/
/* Timer stuff */
/********************************************************/
void init_timer (void)
{
timerH[0]=timerH[1]=0;
time_ptr = (unsigned char *)&time;
INTCONbits.TMR0IE = 1;
// Inlined, we need to change TMR0H before TMR0L (see datasheet)
TMR0H = 0;
TMR0L = 0;
T0CON = 0x07;// 16 bit timer, 1:256 prescaler, not started
INTCON2bits.TMR0IP =1; //TMR0 High Priority
}
void start_timer (void)
{
T0CONbits.TMR0ON = 1;
}
void stop_timer (void)
{
T0CONbits.TMR0ON = 0;
TMR0H = 0;
TMR0L = 0;
timerH[0]=timerH[1]=0;
}
//********************************************************
// Sampling stuff *
//********************************************************
void get_time (void)
{
// TODO do we need 32bits precision ?
// In that case we need to enable the tmr0 overflow interrupt,
// and increment a 16bits-wide counter in the interrupt routine.
//
*time_ptr = TMR0L;
*(time_ptr + 1) = TMR0H;
*(time_ptr + 2) = timerH[0];
*(time_ptr + 3) = timerH[1];
}
void get_sample (void)
{
// copy the mesured position to a buffer to calculate pid
mesured_position_buf[0] = mesured_position[0];
mesured_position_buf[1] = mesured_position[1];
}
void get_derivative_sample (void)
{
old_derivative[0] = new_derivative[0];
old_derivative[0] = new_derivative[0];
new_derivative[0] = error_term[0];
new_derivative[1] = error_term[1];
derivative_term[0] = new_derivative[0] - old_derivative[0];
derivative_term[1] = new_derivative[1] - old_derivative[1];
}
//*********************************************************
//* Pid stuff *
//*********************************************************
void desired_position_accel (void)
{
long temps = (time-time_offset[index]);
desired_velocity[index] = asked_accel[index]*(temps >> 16);
if(desired_velocity[index] >= (asked_velocity[index] >> 16)) {
state[index] = STATE_CNST;
desired_position[index] = asked_velocity[index] * (temps >> 17);
stop_distance[index] = desired_position[index];
time_to_stop[index] = (time-time_offset[index]);
}
else
desired_position[index] = desired_velocity[index] * (temps >> 1);
}
void desired_position_cnst (void)
{
long temps = (time-time_offset[index]);
desired_position[index] = stop_distance[index] +
asked_velocity[index]*((temps - time_to_stop[index]) >> 16);
if (desired_position[index] >= asked_position[index] - stop_distance[index] ) {
time_to_stop[index] = temps;
stop_distance[index] = desired_position[index];
state[index] = STATE_DECEL;
}
}
void desired_position_decel (void)
{
long temps = (time-time_offset[index]) - time_to_stop[index];
desired_position[index] = stop_distance[index] + asked_velocity[index]*(temps >> 16)- asked_accel[index] * (temps >> 16) * (temps>> 1);
if (desired_position[index] >= asked_position[index]) {
desired_position[index] = asked_position[index];
state[index] = STATE_STOP;
}
}
void calculate_desired_position (void)
{
switch (state[index]) {
case STATE_ACCEL:
desired_position_accel();
break;
case STATE_CNST:
desired_position_cnst();
break;
case STATE_DECEL:
desired_position_decel();
break;
default:
return;
}
}
void calculate_error_term (void)
{
position_difference = desired_position[index] - mesured_position_buf[index];
error_term[index] = ( int ) position_difference ;
// if position_difference not on 16 bits, satursate error_term...
// TODO check that
if (error_term[index] != position_difference)
error_term[index] = (position_difference < 0) ? -0x7fff : 0x7fff;
integration_term_buffer = integration_term[index] + error_term[index];
integration_term[index] = (short long)(integration_term_buffer);
if(integration_term[index] != integration_term_buffer){
integration_term[index] = (integration_term_buffer < 0) ? -0x7fffff:0x7fffff ;
//DEBUG
//SEND_PID_STRING(string_int_term_of);
}
// TODO The integration limit is not yet done
}
void do_pid (void)
{
if (state[index] == STATE_STOP) {
if (desired_position[index] >= mesured_position[index]) {
control_terms[index]=0;
return;
}
}
control_term_buffer = coef_prop*error_term[index] + coef_deriv*(derivative_term[index]) + coef_integ*( (int) (integration_term[index] >> 8) );
control_terms[index] = (int)(control_term_buffer);
if(control_terms[index] != control_term_buffer){
control_terms[index] = (control_term_buffer < 0)? -0x7fff : 0x7fff;
//DEBUG
//SEND_PID_STRING(string_control_overflow);
}
}
//*************************************************************
// Update pwms *
//*************************************************************
// Called only once
void update_pwms(void)
{
// TODO check signs
if (control_terms[0]<0) {
CCPR1L = ((-control_terms[0])>>7); // TODO is it only 7 ? signed -> unsigned...
pwm_signs.sign0 = 0;
}
else {
CCPR1L = (control_terms[0]>>7);
pwm_signs.sign0 = 1;
}
if (control_terms[1]<0) {
ECCPR1L = ((-control_terms[1])>>7); // TODO is it only 7 ? signed -> unsigned...
pwm_signs.sign1 = 0;
}
else {
ECCPR1L = (control_terms[1]>>7);
pwm_signs.sign1 = 1;
}
}
void pid_main_loop (void)
{
doMainLoop:
while (!(start_motor[0]||start_motor[1]))
Nop();
status = STATUS_BUSY;
sample_counter = 0;
start_timer();
doSample:
for (index = 0; index < 2; ++index) {
if (start_motor[index]) {
state[index] = STATE_ACCEL;
get_time();
time_offset[index] = time;
signalRegister_LSB[index].traj_complete = 0;
signalRegister_LSB[index].motor_off = 0;
}
start_motor[index] = 0;
}
get_time();
sample_time=time+8; // next sample time. prescaler = 256 -> 8*256 = 2048
get_sample();
++sample_counter;
for (index = 0; index < 2; ++index) {
calculate_desired_position();
calculate_error_term();
}
if (sample_counter == derivative_sample) {
sample_counter = 0;
get_derivative_sample();
}
for (index = 0; index < 2; ++index) {
do_pid();
}
//update pwms : try to synchronise as much as possible
update_pwms();
traj_finished = 1;
for (index = 0; index < 2; ++index) {
if (state[index] == STATE_STOP && control_terms[index] == 0) {
signalRegister_LSB[index].traj_complete = 1;
signalRegister_LSB[index].motor_off = 1;
}
else traj_finished = 0;
}
//DEBUG
if (counter == 1) {
counter = 0;
//SEND_PID_STRING(string_control);
//SEND_PID_INT(control_terms[0]);SEND_PID_BYTE('\t');
//SEND_PID_INT(control_terms[1]);SEND_PID_STRING(new_line);
SEND_PID_STRING(string_des_pos);
SEND_PID_LONG_HEXA(desired_position[0]);SEND_PID_BYTE('\t');
SEND_PID_LONG_HEXA(desired_position[1]);SEND_PID_STRING(new_line);
SEND_PID_STRING(string_des_vel);
SEND_PID_LONG_HEXA(desired_velocity[0]);SEND_PID_BYTE('\t');
SEND_PID_LONG_HEXA(desired_velocity[1]);SEND_PID_STRING(new_line);
SEND_PID_STRING(string_pwm);
SEND_PID_UCHAR_HEXA(PWM0_DUTY_CYCLE);SEND_PID_BYTE('\t');
SEND_PID_UCHAR_HEXA(PWM1_DUTY_CYCLE);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_des_ask);
//SEND_PID_LONG_HEXA((asked_velocity[0] >> 16));SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA((asked_velocity[1] >> 16));SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_int_term);
//SEND_PID_LONG_HEXA(integration_term[0]);SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA(integration_term[1]);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_control_term);
//SEND_PID_LONG_HEXA(control_terms[0]);SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA(control_terms[1]);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_ask_acc);
//SEND_PID_LONG_HEXA(asked_accel[0]>>16);SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA(asked_accel[1]>>16);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_pos_dif);
//SEND_PID_LONG_HEXA(position_difference);
//SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_error_term);
//SEND_PID_INT(error_term[0]);SEND_PID_BYTE('\t');
//SEND_PID_INT(error_term[1]);SEND_PID_STRING(new_line);
//SEND_PID_STRING(string_int_term);
//SEND_PID_LONG_HEXA(integration_term[0]);SEND_PID_BYTE('\t');
//SEND_PID_LONG_HEXA(integration_term[1]);SEND_PID_STRING(new_line);
SEND_PID_STRING(string_state);
SEND_PID_UCHAR_HEXA(state[0]);SEND_PID_BYTE('\t');
SEND_PID_UCHAR_HEXA(state[1]);SEND_PID_STRING(new_line);SEND_PID_STRING(new_line);
}
counter++;
if (!traj_finished) {
while (time < sample_time){
get_time();
}
goto doSample;
}
status = STATUS_MOTOR_OFF | STATUS_TRAJECT_END;
stop_timer();
goto doMainLoop;
}
#endif /*__PID__H*/
