 |
 |
| View previous topic :: View next topic |
| Author |
Message |
Spukkers
Joined: 18 Jun 2009
Posts: 2
|
| Need help with motion profile program |
 Posted: Mon Jun 29, 2009 2:39 am |
 |
|
Hello,
I need help with my C-program, I found a source code from microchip (AN696). but when I try to change the program to a ccs program, the program simply doesn't work properly.
| Code: |
void UpdTraj (void);
void SetupMove (void);
void UpdPos (void);
void CalcError (void);
void CalcPID (void);
void PicInit(void);
#define TotDist 30000
#define HalfDist TotDist/2
#define Vmax 40
#define DIST 0
#define VEL 1
#define ACCEL 2
#define TIME 3
unsigned int8 firstseg, lastseg, segnum, parameter;
int16 dtime, integral, kp, ki, kd, vlim, mvelocity, upcount;
int16 segment1[12][4];
int32 position, mposition, fposition, flatcount;
struct Status
{
int1 phase;
int1 neg_move;
int1 motion;
int1 saturated;
int1 bit4;
int1 bit5;
int1 run;
int1 loop;
} stat;
union LNG
{
int32 l;
unsigned int32 ul;
int16 i [2];
unsigned int16 ui [2];
int8 b [4];
unsigned int8 ub [4];
};
union LNG
temp, accel, u0, ypid, velact, phase1dist;
#INT_TIMER2
void servo_isr(void)
{
UpdTraj();
SetupMove();
UpdPos();
CalcError();
CalcPID();
clear_interrupt(INT_TIMER2);
}
void main()
{
PicInit();
printf("\n\rSTART\n\r");
//PWM settings
setup_ccp2(CCP_OFF );
output_low(PIN_C1);
output_high(PIN_B5);
set_pwm1_duty(0);
setup_ccp1(CCP_PWM);
while(1)
{
if(kbhit())
switch (getc())
{
case 'q':
{
printf("\n\rAAN");
stat.run =1;
stat.motion = 1;
stat.loop = 0;
firstseg = 0;
lastseg = 0;
segnum = 0;
set_timer1(0);
enable_interrupts(INT_TIMER2);
break;
}
case 'w':
{
break;
}
default:;
break;
}
}
}
void UpdTraj (void)
{
if (stat.motion && !stat.saturated)
{//Do motion profile
if(!stat.phase)
{
//printf("\n\r%lu\n\r",velact.ui[1]);
if(velact.ui[1] < vlim)
{
velact.ul = velact.ul + accel.ul;
}
else
{
flatcount++;
//printf("\n\r%lu\n\r",flatcount);
}
temp.ul = velact.ul;
if (velact.ui[0] == 0x8000)
{
if (!(velact.ub[2] & 0x01))
{
temp.ui[1]++;
}
else ;
}
else if (velact.ui[0] > 0x8000)
{
temp.ui[1]++;
}
else;
phase1dist.ul = phase1dist.ul - (unsigned int32)temp.ui[1];
if (stat.neg_move)
{
position = position - (unsigned int32)temp.ui[1];
}
else
{
position = position + (unsigned int32)temp.ui[1];
}
if (phase1dist.l <= 0)
{
stat.phase = 1;
}
}
else
{
if (flatcount)
{
flatcount--;
}
else if (velact.ul)
{
velact.ul = velact.ul - accel.ul;
if (velact.i[1] <0)
{
velact.l = 0;
}
}
else
{
if (dtime)
{
dtime--;
}
else
{
stat.motion = 0;
position = fposition;
}
}
temp.ul = velact.ul;
if (velact.ui[0]==0x8000)
{
if (!(velact.ub[2] & 0x01))
{
temp.ui[1]++;
}
else;
}
else if (velact.ui[0]>0x8000)
{
temp.ui[1]++;
}
else;
if (stat.neg_move)
{
position = position - (unsigned int32) temp.ui[1];
}
else
{
position = position + (unsigned int32) temp.ui[1];
}
}
}
else
{//load next segment
if (stat.run && !stat.motion)
{
if (segnum < firstseg) segnum = firstseg;
if (segnum > lastseg)
{
segnum = firstseg;
if(!stat.loop) stat.run = 0;
}
else
{
SetupMove();
segnum++;
}
}
else;
}
}
void SetupMove (void)
{
if (segnum < 12)
{
phase1dist.i[0] = segment1[segnum][DIST];
vlim = segment1[segnum][VEL];
accel.i[0] = segment1[segnum][ACCEL];
dtime = segment1[segnum][TIME];
}
else ;
phase1dist.b[2]=phase1dist.b[1];
phase1dist.b[1]=phase1dist.b[0];
phase1dist.b[0]=0;
if (phase1dist.b[2] & 0x80)
{
phase1dist.b[3] = 0xFF;
}
else
{
phase1dist.b[3] = 0;
}
accel.b[3]=0;
accel.b[2]=accel.b[1];
accel.b[1]=accel.b[0];
accel.b[0]=0;
temp.l = position;
if (temp.ub[0]>0x7F)
{
temp.l = temp.l + 0x100;
}
temp.ub[0]=0;
position = temp.l;
fposition = position + phase1dist.l;
if (phase1dist.b[3] & 0x80)
{
stat.neg_move = 1;
phase1dist.l = -phase1dist.l;
}
else
{
stat.neg_move = 0;
}
phase1dist.l >>= 1;
velact.l = 0;
flatcount = 0;
stat.phase = 0;
if (accel.l && vlim)
{
stat.motion = 1;
}
}
void UpdPos (void)
{
mvelocity = upcount;
upcount = get_timer1();
mvelocity = upcount - mvelocity;
mposition = mposition + mvelocity;
//printf("\n\r%lu-%lu\n\r",mvelocity, mposition);
}
void CalcError (void)
{
temp.l = position;
temp.b[0] = 0;
u0.l = mposition - temp.l;
u0.b[0] = u0.b[1];
u0.b[1] = u0.b[2];
u0.b[2] = u0.b[3];
if(u0.b[2] & 0x80)
{
u0.b[3] = 0xFF;
if((u0.ui[1] != 0xFFFF) || !(u0.ub[1] & 0x80))
{
u0.ui[1] = 0xFFFF;
u0.ui[0] = 0x8000;
}
else;
}
else
{
u0.b[3] = 0x00;
if((u0.ui[1] != 0x0000) || (u0.ub[1] & 0x80))
{
u0.ui[1]=0x0000;
u0.ui[0]=0x7FFF;
}
else;
}
}
void CalcPID (void)
{
ypid.l = (int32)u0.i[0]*(int32)kp;
if(!stat.saturated)
{
integral = integral + u0.i[0];
}
if(ki)
{
ypid.l = ypid.l + (int32)integral*(int32)ki;
}
if(kd)
{
ypid.l = ypid.l + (int32)mvelocity*(int32)kd;
}
/*
ypid.i[0] = u0.i[0]*kp;
ypid.ub[2]=??????;
ypid.ub[3]=??????;
if(!stat.saturated)
{
integral = integral + u0.i[0];
}
if(ki)
{
temp.i[0] = integral * ki;
temp.ub[2] = ?????;
temp.ub[3] = ?????;
ypid.l = ypid.l + temp.l;
}
if (kd)
{
temp.i[0] = mvelocity * kd;
temp.ub[2] = ?????;
temp.ub[3] = ?????;
ypid.l = ypid.l + temp.l;
}
*/
if (ypid.ub[3] & 0x80)
{
if ((ypid.ub[3] < 0xFF) || !(ypid.ub[2] & 0x80))
{
ypid.ui[1] = 0xFF80;
ypid.ui[0] = 0x0000;
}
else;
}
else
{
if(ypid.ub[3] || (ypid.ub[2] > 0x7F))
{
ypid.ui[1] = 0x007F;
ypid.ui[0] = 0xFFFF;
}
else;
}
ypid.b[0] = ypid.b[1];
ypid.b[1] = ypid.b[2];
stat.saturated = 0;
if(ypid.i[0] > 1000)
{
ypid.i[0] = 1000;
stat.saturated = 1;
}
if (ypid.i[0]< 20)
{
ypid.i[0]= 20;
stat.saturated = 1;
}
set_pwm1_duty(ypid.i[0]);
printf("\n\r%lu\n\r",ypid.i[0]);
}
void PicInit(void)
{
set_tris_a(0b11111100);
set_tris_b(0b11001111);
set_tris_c(0b10111001);
set_tris_d(0b11111111);
set_tris_e(0b00000000);
set_tris_f(0b11111111);
set_tris_g(0b11111111);
#use fast_io(a)
//#use fast_io(b)
//#use fast_io(c)
#use fast_io(d)
#use fast_io(e)
#use fast_io(f)
#use fast_io(g)
output_low(PIN_B5);
output_low(PIN_C1); //PWM signale laag maken
output_low(PIN_C2); //PWM signale laag maken
setup_timer_0(RTCC_EXT_L_TO_H);
setup_timer_1 ( T1_EXTERNAL_SYNC|T1_DIV_BY_1 );
setup_timer_2 ( T2_DIV_BY_1, 255, 16);
enable_interrupts(GLOBAL);
delay_ms(1000);
output_high(PIN_B4);//RX en TX signaal omzetten zodat de UART tool gebruikt kan worden
set_timer1(0);
firstseg = 0; // Initialize motion segment
lastseg = 0; // variables
segnum = 0;
parameter = 0; // Motion segment parameter#
stat.phase = 0; // Set flags to 0.
stat.saturated = 0;
stat.motion = 0;
stat.run = 0;
stat.loop = 0;
stat.neg_move = 0;
dtime = 0;
integral = 0;
vlim = 0;
mvelocity = 0;
upcount = 0;
temp.l = 0;
accel.l = 0;
u0.l = 0;
ypid.l = 0;
velact.l = 0;
phase1dist.l = 0;
position = 0;
mposition = 0;
fposition = 0;
flatcount = 0;
kp= 50;
ki= 0;
kd= 0;
segment1[0][DIST] = 30000;
segment1[0][VEL] = 18;
segment1[0][ACCEL] = 2;
segment1[0][TIME] = 12;
}
|
Can anyone help me please.
Who has some experience with a servo system or with the Application note from microchip(AN696).
Regard Spukkers |
|
 |
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Mon Jun 29, 2009 1:36 pm |
|
|
| Quote: |
when I try to change the program to a ccs program, the program simply doesn't work properly. |
You implied that you had converted it to CCS, but that it would not run
correctly on a PIC. In fact, it doesn't compile. You need to work
on it some more, so that it compiles without errors.
Also, when you post code, post the #include, #fuses, and #use delay()
statements so the program is complete. |
|
|
Spukkers
Joined: 18 Jun 2009
Posts: 2
|
| Working code changed AN696 |
| Posted: Wed Jul 01, 2009 5:39 am |
|
|
Hello,
I've got our code to work.
with the folliwing code we can control a sliding door with a brushed dc motor.
The pwm controls a l298n to drive the motor.
the motor has a encoder to get the postition of the door. like AN696 except we adjust the program and hardware a litle bit, to use one timer for the pulsecount. We cummunicatie over ethernet (UDP).
| Code: |
#define STACK_USE_ICMP 1 //ethernet settings
#define STACK_USE_ARP 1
#define STACK_USE_UDP 1
#define MAX_SOCKETS 2 //the program needs two sockets, one for sending
#define MAX_UDP_SOCKETS 2 //and one for receiving data
#include "ccstcpip.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#CASE // make compiler case sensative
#define UDP_SOURCE_PORT 1024 //set port to receive data from
#define UDP_DESTINATION_PORT 1025 //set port to send data to
#define UDP_NUM_LISTEN_SOCKETS 1 //set maximum number of computers to communicate with
#define enable PIN_B5 //define enable for the l298n, to make programming easier
#bit INTEDG3 = 0xFF1.3 //define bit 3 of register 0xFF1 as INTEDG3,
//this bit controls the edge triggring of the external interrupt,H_to_L or L_to_H
UDP_SOCKET last_rx_socket=INVALID_UDP_SOCKET; //sets the endpoint for the maximum of the UDP sockets
//functions declerations
void OpenInit(void);
void UpdTraj (void);
void UpdPos (void);
void CalcError (void);
void CalcPID (void);
void PicInit(void);
void UDPRxTask(void);
void UDPTxTask(void);
signed int32 Teach(void);
//create variables which can be used trough the hole program
int8 ibuffer[4]={255,255,255,255}, obuffer[2]={255,255};
signed int16 dtime, integral, mvelocity, upcount;
signed int32 position, mposition, fposition, flatcount, vlim;
signed int32 accel, temp, u0, ypid, velact;
signed int32 phase1dist, halfdist, halfdisttemp;
signed int32 x0=0, y0=0, z0=0, u=0;
signed int32 x1, y1, z1, u1;
signed int32 Kr=1, Ti=100, Td=0; // parameters of the PID-controller
signed int32 U, dt=0.667;
signed int32 timer1old=0, timer1oldold=0,pulses;
struct Status //create a structure to maintain the statusses of the program
{
int1 phase;
int1 open;
int1 closed;
int1 motion;
int1 saturated;
int1 init;
int1 manual;
int1 run;
int1 summerwinter;
} stat;
#INT_EXT3 FAST //use external interrupt3 for the lightsensor
void light_sens()
{
clear_interrupt(INT_TIMER2);
disable_interrupts(INT_TIMER2);
set_pwm3_duty(0);
setup_ccp3(CCP_OFF);
output_low(PIN_D1);
output_high(enable);
set_pwm1_duty(0);
delay_ms(1000);
setup_ccp1(CCP_PWM);
stat.phase = 0; // Set flags to 0.
stat.open = 0;
stat.saturated = 0;
stat.motion = 0;
mvelocity = 0;
upcount = 0;
temp = 0;
u0 = 0;
x0=0; y0=0; z0=0; u=0; x1=0; y1=0; z1=0; u1=0;
velact = 0;
position = 0;
mposition = 0;
fposition = 0;
flatcount = 0;
stat.run = 1;
stat.motion = 1;
obuffer[1] = 0;
phase1dist = get_timer1();
phase1dist = phase1dist * 500;
set_timer1(0);
printf("\n\rphase1dist=%ld\n\r",phase1dist);
dtime = 3000;
enable_interrupts(INT_TIMER2);
disable_interrupts(INT_EXT3);
clear_interrupt(INT_EXT3);
}
#INT_TIMER2 //interrupt service routine, enters every 16 pwm periods
void servo_isr()
{
UpdTraj();
UpdPos();
CalcError();
CalcPID();
clear_interrupt(INT_TIMER2);
}
void main()
{
PicInit();
MACAddrInit(); //initialize the mac adress of the pic
IPAddrInit(); //initialize the ip-adress of the pic
StackInit();
while(1)
{
StackTask();
UDPRxTask();
switch (ibuffer[0])
{
case 0: //open/close automatic
{
if (!stat.manual && stat.closed ) //if door is not opened manual and door is closed
{
dtime = ibuffer[1] * 1500;
printf("\n\rdtime=%ld\n\r",dtime);
OpenInit();
enable_interrupts(INT_TIMER2);
}
break;
}
case 1: //open manual
{
if (!stat.manual) //if door is not opened manual
{
stat.manual = 1;
OpenInit();
enable_interrupts(INT_TIMER2);
}
break;
}
case 2: //close door manual
{
if (stat.manual) //if door is opened manual
{
stat.manual = 0;
dtime = 0 ;
enable_interrupts(INT_TIMER2);
}
break;
}
case 3: //adjust the acceleration and decceleration
{
accel = ibuffer[1];
printf("\n\raccel=%u\n\r",accel);
break;
}
case 4: //adjust the velocity limit
{
vlim = (signed int32)ibuffer[1] * 1000;
printf("\n\rvlim=%ld\n\r",vlim);
break;
}
case 5: //initialize the controller
{
accel = ibuffer[1]; //set acceleration/decceleration
vlim = (signed int32)ibuffer[2] * 1000; //set velocity limit
printf("\n\rvlim=%ld\n\r",vlim);
printf("\n\raccel=%u\n\r",accel);
halfdisttemp = Teach();
if (!stat.manual)//if door is not opened manually set summer or wintertime
{
stat.summerwinter=ibuffer[3];
if (stat.summerwinter)
{
halfdist = halfdisttemp * 500;
printf("\n\Summertime\n\r");
}
else
{
halfdist = halfdisttemp * 250;
printf("\n\Wintertime\n\r");
}
}
break;
}
case 7:
{
if (!stat.manual)//if not opened manually, set summer or wintertime
{
stat.summerwinter=ibuffer[1];
if (stat.summerwinter)
{
halfdist = halfdisttemp * 500; //door will open full
printf("\n\Summertime\n\r");
}
else
{
halfdist = halfdisttemp * 250; //door will open half
printf("\n\Wintertime\n\r");
}
}
break;
}
default:;
break;
}
if (stat.closed) //if door is closed, and pulses are count, generate error to local pc
{
if (get_timer1() > 20)
{
obuffer[0]=1;
obuffer[1]=255;
UDPTxTask();
printf("\n\rALARM!!!\n\rGot pulses\n\r",);
set_timer1(0);
}
}
ibuffer[0]=255;//reset the input buffer to prevent that cases are re-entered
ibuffer[1]=255;
ibuffer[2]=255;
ibuffer[3]=255;
}
}
/*********************************************************************
* Function: void OpenInit(void)
*
* PreCondition: None
*
* Input: None
*
* Output: Initializations are made to open the door
*
* Side Effects: None
*
* Note: This function must be called before open the door
*
********************************************************************/
void OpenInit(void)
{
setup_ccp3(CCP_OFF );
output_low(PIN_D1);
output_high(enable);
set_pwm1_duty(0);
setup_ccp1(CCP_PWM);
stat.phase = 0; // Set flags to 0.
stat.open = 0;
stat.closed = 0;
stat.saturated = 0;
stat.motion = 0;
mvelocity = 0;
upcount = 0;
temp = 0;
u0 = 0;
velact = 0;
position = 0;
mposition = 0;
fposition = 0;
flatcount = 0;
phase1dist = halfdist;
stat.run =1;
stat.motion = 1;
obuffer[1] = 0;
set_timer1(0);
}
/*********************************************************************
* Function: void UpdTraj(void)
*
* PreCondition: OpenInit() has to be called
*
* Input: None
*
* Output: position where the door supposed to be
*
* Side Effects: None
*
* Note: This function calculates where door supposed to be
*
********************************************************************/
void UpdTraj (void)
{
if (stat.motion && !stat.saturated)
{//Do motion profile
//if (stat.open)printf("\n\rQ\n\r");
if(!stat.phase)
{
//printf("\n\rh\n\r");
if(velact < vlim)
{
velact = velact + accel;
}
else
{
flatcount++;
//printf("\n\r%lu\n\r",flatcount);
}
temp = velact;
phase1dist = phase1dist - temp;
position = position + temp;
if (phase1dist <= 0)
{
stat.phase = 1;
phase1dist = 0;
}
}
else
{
if (flatcount)
{
flatcount--;
}
else if (velact)
{
velact = velact - accel;
if (velact < 1)
{
velact = 0;
}
}
else
{
stat.motion = 0;
if (stat.open)
{
printf("\n\r%lu\n\r", get_timer1());
set_pwm3_duty(0);
setup_ccp3(CCP_OFF );
output_low(PIN_D1);
obuffer[0]=0;
obuffer[1]=0;
UDPTxTask();
stat.closed = 1;
set_timer1(0);
disable_interrupts(INT_TIMER2);
disable_interrupts(INT_EXT3);
}
}
temp = velact;
position = position + temp;
}
}
else
{
if (!obuffer[1])
{
obuffer[0]=0;
obuffer[1]=1;
UDPTxTask();
}
if (!stat.manual)
{
if (dtime)
{
dtime--;
}
else if (stat.run && !stat.motion && !stat.open)
{
//stat.run = 0;
stat.open = 1;
phase1dist = halfdist;
printf("\n\r%lu\n\r", get_timer1());
position = 0;
mposition = 0;//position;
set_timer1(0);
stat.phase = 0;
stat.saturated = 0;
mvelocity = 0;
upcount = 0;
temp = 0;
u0 = 0;
velact = 0;
mposition = 0;
flatcount = 0;
stat.run =1;
stat.motion = 1;
setup_ccp1(CCP_OFF );
output_low(PIN_C2);
set_pwm3_duty(0);
setup_ccp3(CCP_PWM);
enable_interrupts(INT_EXT3);
}
}
else if (stat.manual)
{
setup_ccp1(CCP_OFF );
disable_interrupts(INT_TIMER2);
}
}
}
/*********************************************************************
* Function: void UpdPos(void)
*
* PreCondition: OpenInit() has to be called
*
* Input: None
*
* Output: position where the door is at the moment
*
* Side Effects: None
*
* Note: This function reads where door supposed is
*
********************************************************************/
void UpdPos (void)
{
mvelocity = upcount;
upcount = get_timer1() * 1000;
mvelocity = upcount - mvelocity;
mposition = mposition + mvelocity;
}
/*********************************************************************
* Function: void CalcError(void)
*
* PreCondition: OpenInit() has to be called
*
* Input: None
*
* Output: Position error
*
* Side Effects: None
*
* Note: This function calculates the positioning error
*
********************************************************************/
void CalcError (void)
{
u0 = position - mposition;
//printf("\n\r%ld - %ld\n\r",position, mposition);
}
/*********************************************************************
* Function: void CalcPID(void)
*
* PreCondition: OpenInit() has to be called
*
* Input: Positioning error u0
*
* Output: Pwm dutycycle
*
* Side Effects: None
*
* Note: The PID controller calculates the new dutycycle
*
********************************************************************/
void CalcPID (void)
{
//printf("\n\ru0=%lu\n\r",u0.l);
x0 = (Kr*u0)/1000; // P-action with Kr=100
y1 = y0 + x0/Ti*dt;
z1 = y0 + x0; // I-action with timeconstant Ti= 10
u1 = z0 + Td*(z1-z0)/dt; // D-action with timeconstant Td= 1
y0=y1;
z0=z1;
u=u1;
stat.saturated = 0;
if(u1 < 0) //limit the calculated new dutycycle to zero to prevent strange behaviour
{
stat.saturated = 1;
u1 = 0; // limit u1
}
if(u1 > 1023) //limit the calculated new dutycycle to 1023 to prevent strange behaviour
{
stat.saturated = 1;
u1 = 1023; // limit u1
}
if (!stat.open) //if door is opening, set pwm duty cycle to ccp1
{
set_pwm1_duty(u1);
}
else //if door is closing, set pwm duty cycle to ccp3
{
set_pwm3_duty(u1);
}
}
/*********************************************************************
* Function: void PicInit(void)
*
* PreCondition: None
*
* Input: None
*
* Output: None
*
* Side Effects: None
*
* Note: Has to be called before any other function
*
********************************************************************/
void PicInit(void)
{
set_tris_a(0b11111100);
*0xF92=*0xF92 & 0xFC; //a0 and a1 output
set_tris_b(0b11001111);
set_tris_c(0b10111011);
set_tris_d(0b11111101);
set_tris_e(0b00000000);
set_tris_f(0b11111111);
set_tris_g(0b11111111);
#use fast_io(a)
#use fast_io(b)
#use fast_io(c)
#use fast_io(d)
#use fast_io(e)
#use fast_io(f)
#use fast_io(g)
output_low(enable);
output_low(PIN_D1); //lower the pwm signals to prevent drifting of the door
output_low(PIN_C2); //lower the pwm signals to prevent drifting of the door
setup_adc(ADC_CLOCK_INTERNAL); //setup AD-Converter for Current measurement
setup_adc_ports(AN0_TO_AN4);
set_adc_channel(4); //set ADC to channel 4
setup_timer_1 ( T1_EXTERNAL_SYNC); //setup timer1 as counter for the pulscount
setup_timer_2 ( T2_DIV_BY_1, 255, 16); //setup timer2 as clocksource for PWM and generate an interrupt every 16 dutyc.
enable_interrupts(INT_EXT3); //setup external interrupt for light sensor
INTEDG3 = 0; //falling edge triggering
enable_interrupts(GLOBAL); //enable all interrupts
delay_ms(1000); //delay to get a stable signal to switch from programming mode to UART tool
output_high(PIN_B4);//RX en TX signaal omzetten zodat de UART tool gebruikt kan worden
stat.manual = 0;
accel = 0;
vlim = 0;
stat.summerwinter = 1; //summertime
stat.closed = 1;
halfdist = 0;
set_timer1(0);
printf("Software was compiled on: ");
printf(__DATE__);
printf(" at ");
printf(__TIME__);
printf(" \n\r ");
}
/*********************************************************************
* Function: void UDPRxTask(void)
*
* PreCondition: The following functions have to be called:
* PicInit()
* MACAddrInit()
* IPAddrInit()
* StackInit()
* StackTask()
*
* Input: None
*
* Output: ibuffer[]
*
* Side Effects: None
*
* Note: None
*
********************************************************************/
void UDPRxTask(void) {
static enum {UDP_RX_OPEN=0, UDP_RX_LISTEN=1, UDP_RX_BLAH=2}
state[UDP_NUM_LISTEN_SOCKETS]={0};
static UDP_SOCKET socket[UDP_NUM_LISTEN_SOCKETS]={INVALID_UDP_SOCKET};
int8 i,z=0, c;
for (i=0;i<UDP_NUM_LISTEN_SOCKETS;i++) {
if (socket[i]==INVALID_UDP_SOCKET)
state[i]=UDP_RX_OPEN;
switch (state[i]) {
case UDP_RX_OPEN:
socket[i]=UDPOpen(UDP_SOURCE_PORT,NULL,INVALID_UDP_SOCKET);
if (socket[i]!=INVALID_UDP_SOCKET) {
printf("\r\nUDP SOCKET %U OPEN FOR LISTENING", socket[i]);
state[i]=UDP_RX_LISTEN;
}
break;
case UDP_RX_LISTEN:
if (UDPIsGetReady(socket[i])) {
last_rx_socket=socket[i];
printf("\r\nGOT [IP:%U.%U.%U.%U",
UDPSocketInfo[socket[i]].remoteNode.IPAddr.v[0],
UDPSocketInfo[socket[i]].remoteNode.IPAddr.v[1],
UDPSocketInfo[socket[i]].remoteNode.IPAddr.v[2],
UDPSocketInfo[socket[i]].remoteNode.IPAddr.v[3]
);
printf(" SP:%LU DP:%LU S:%U] - ",
UDPSocketInfo[socket[i]].remotePort,
UDPSocketInfo[socket[i]].localPort,
socket[i]
);
printf("\f%U.%U.%U.%U\n",
UDPSocketInfo[socket[i]].remoteNode.IPAddr.v[0],
UDPSocketInfo[socket[i]].remoteNode.IPAddr.v[1],
UDPSocketInfo[socket[i]].remoteNode.IPAddr.v[2],
UDPSocketInfo[socket[i]].remoteNode.IPAddr.v[3]
);
while(UDPGet(&c)) {
ibuffer[z]=c;
printf("%u",ibuffer[z]);
z++;
}
}
break;
}
}
}
/*********************************************************************
* Function: void UDPTxTask(void)
*
* PreCondition: The following functions have to be called:
* PicInit()
* MACAddrInit()
* IPAddrInit()
* StackInit()
* StackTask()
*
* Input: obuffer[]
*
* Output: None
*
* Side Effects: None
*
* Note: None
*
********************************************************************/
void UDPTxTask(void) {
static enum {UDP_TX_OPEN=2, UDP_TX_WAIT=0, UDP_TX_DEBOUNCE=1,
UDP_TX_ISREADY=3} state=0;
static int8 tx_socket;
switch (state) {
case UDP_TX_WAIT:
if (last_rx_socket!=INVALID_UDP_SOCKET) {
state=UDP_TX_OPEN;
}
case UDP_TX_OPEN:
tx_socket=UDPOpen(UDP_SOURCE_PORT, &UDPSocketInfo[last_rx_socket].remoteNode, UDP_DESTINATION_PORT);
if (tx_socket==INVALID_UDP_SOCKET)
{
printf("\r\nCan't open UDP socket for transmit");
state=UDP_TX_WAIT;
}
else
{
state=UDP_TX_ISREADY;
}
case UDP_TX_ISREADY:
if (UDPIsPutReady(tx_socket)) {
printf(UDPPut,"%u%u",obuffer[0],obuffer[1]);
UDPFlush();
UDPClose(tx_socket);
state=UDP_TX_WAIT;
}
break;
}
}
/*********************************************************************
* Function: void UDPTxTask(void)
*
* PreCondition: PicInit() has to be called
*
* Input: Speed and MaxCurrent has to be defined
*
* Output: total distance in pulses
*
* Side Effects: None
*
* Note: None
*
********************************************************************/
signed int32 Teach(void)
{
int8 nCurrent=0, i, maxcurr = 80, speed = 100;
signed int32 totdist;
setup_ccp3(CCP_OFF );
output_low(PIN_D1);
output_high(enable);
set_pwm1_duty(0);
setup_ccp1(CCP_PWM);
for (i=1;i<=speed;++i)
{
set_pwm1_duty(i);
delay_ms(30);
nCurrent = read_adc();
if (nCurrent > maxcurr)i=speed;
}
while (nCurrent < maxcurr)
{
nCurrent = read_adc();
}
obuffer[0]=0;
obuffer[1]=1;
UDPTxTask();
set_pwm1_duty(0);
setup_ccp1(CCP_OFF);
output_low(PIN_C2);
delay_ms(1000);
set_pwm3_duty(0);
setup_ccp3(CCP_PWM);
set_timer1(0);
for (i=1;i<=speed;++i)
{
set_pwm3_duty(i);
delay_ms(30);
nCurrent = read_adc();
if (nCurrent > maxcurr)i=speed;
}
while (nCurrent < maxcurr)
{
nCurrent = read_adc();
}
totdist = get_timer1();
set_pwm3_duty(0);
setup_ccp3(CCP_OFF);
output_low(PIN_D1);
obuffer[0]=0;
obuffer[1]=0;
UDPTxTask();
return(totdist);
}
|
| Code: |
//////////////////////////////////////////////////////////////////////////////
//
// ccstcpip.h - Common code shared among all Embedded Internet/Embedded
// Ethernet tutorial book examples.
//
// If you are using a CCS Embedded Ethernet Board (labeled PICENS, which
// has an MCP ENC28J60) then define STACK_USE_CCS_PICENS to TRUE.
//
// If you are using a CCS Embedded Internet Board (labeled PICNET, which
// has a Realtek RTL8019AS and a 56K Modem) then define STACK_USE_CCS_PICNET
// to TRUE.
//
//////////////////////////////////////////////////////////////////////////////
//
// 10/25/06
// - Added STACK_USE_CCS_PICEEC
// - ExampleUDPPacket[] UDP header length fixed
//
// 2008.12.20 - JGSCHMIDT
// - added STACK_USE_CCS_OLXMINI for Olimex Mini-Web board
//
//////////////////////////////////////////////////////////////////////////////
#define STACK_USE_CCS_PICEEC 1
#define STACK_USE_MCPINC 1
#include <18F67J60.h>
#device adc=8
#device HIGH_INTS=TRUE
#use delay(clock=25M)
#fuses NOWDT, NODEBUG, HS, NOIESO, NOFCMEN, PRIMARY, ETHLED
#use rs232(baud=38400,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8)
#include "tcpip/stacktsk.c" //include Microchip TCP/IP Stack
void MACAddrInit(void) {
MY_MAC_BYTE1=0;
MY_MAC_BYTE2=2;
MY_MAC_BYTE3=3;
MY_MAC_BYTE4=4;
MY_MAC_BYTE5=5;
MY_MAC_BYTE6=6;
}
void IPAddrInit(void) {
//IP address of this unit
MY_IP_BYTE1=172;
MY_IP_BYTE2=20;
MY_IP_BYTE3=0;
MY_IP_BYTE4=110;
//network gateway
MY_GATE_BYTE1=172;
MY_GATE_BYTE2=20;
MY_GATE_BYTE3=0;
MY_GATE_BYTE4=1;
//subnet mask
MY_MASK_BYTE1=255;
MY_MASK_BYTE2=255;
MY_MASK_BYTE3=255;
MY_MASK_BYTE4=0;
}
char ExampleIPDatagram[] = {
0x45, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00,
0x64, 0x11, 0x2A, 0x9D, 0x0A, 0x0B, 0x0C, 0x0D,
0x0A, 0x0B, 0x0C, 0x0E
};
char ExampleUDPPacket[] = {
0x04, 0x00, 0x04, 0x01, 0x00, 0x08, 0x00, 0x00,
0x01, 0x02, 0x03, 0x04
};
|
|
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
