 |
 |
| View previous topic :: View next topic |
| Author |
Message |
RUFINOGG
Joined: 15 Jan 2010
Posts: 8
|
| Sort Servomotor Application AN696 Microchip |
 Posted: Mon Dec 09, 2013 11:05 am |
 |
|
I am using the Microchip AN696 application and it works perfectly, but I want to modify it to order a new motor shaft position (destination) is "real" and not the current position plus the value sent by RS232. For more changes I've made have been unable to get it. Has anyone used the application note or other which incorporates a PID that works well?
Thanks |
|
 |
temtronic
Joined: 01 Jul 2010
Posts: 9170
Location: Greensville,Ontario
|
|
| Posted: Mon Dec 09, 2013 2:05 pm |
|
|
| As the AN was not written in CCS C , please post your translation of the AN, then we can show you what to do. |
|
|
RUFINOGG
Joined: 15 Jan 2010
Posts: 8
|
|
| Posted: Mon Dec 09, 2013 6:18 pm |
|
|
This is the complete program translated to CCS:
| Code: |
//
//
File: ZoomAN696.H
//
///////////////////////////////////////////////////////////////////////////////
//////// Program memory: 0x16 Data RAM: 3840 Stack: 31
//////// I/O: 40 Analog Pins: 12
//////// Data EEPROM: 1024
//////// C Scratch area: 00 ID Location: 2000
//////// Fuses: LP,XT,HS,RC,EC,EC_IO,H4,RC_IO,PROTECT,NOPROTECT,IESO,NOIESO
//////// Fuses: NOBROWNOUT,BROWNOUT,WDT1,WDT2,WDT4,WDT8,WDT16,WDT32,WDT64
//////// Fuses: WDT128,WDT,NOWDT,BORV25,BORV27,BORV42,BORV45,PUT,NOPUT,CPD
//////// Fuses: NOCPD,NOSTVREN,STVREN,NODEBUG,DEBUG,NOLVP,LVP,WRT,NOWRT,CPB
//////// Fuses: NOCPB,EBTRB,NOEBTRB,EBTR,NOEBTR,CCP2B3,CCP2C1,WRTD,NOWRTD
//////// Fuses: WRTC,NOWRTC,WRTB,NOWRTB,FCMEN,NOFCMEN,INTRC,INTRC_IO
//////// Fuses: BROWNOUT_SW,BROWNOUT_NOSL,WDT_256,WDT_512,WDT_1024,WDT_2048
//////// Fuses: WDT_4096,WDT_8192,WDT_16384,WDT_32768,LPT1OSC,NOLPT1OSC,MCLR
//////// Fuses: NOMCLR,XINST,NOXINST,BBSIZE1K,BBSIZE2K,BBSIZE4K
///////////////////////////////////////////////////////////////////////////////
#ifdef Debuger1
#ifdef PLLMHz
#fuses H4,noWDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP,DEBUG, WDT128,NOEBTR,BROWNOUT
#else
#fuses HS,noWDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP,DEBUG, WDT128,NOEBTR,BROWNOUT
#endif
#else
#ifdef PLLMHz
#fuses H4,WDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP, noDEBUG, WDT128,NOEBTR,BROWNOUT
#else
#fuses HS,WDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP, noDEBUG, WDT128,NOEBTR,BROWNOUT
#endif
#endif
#ifdef PLLMHz
#use delay(clock=40000000,RESTART_WDT) //10.000.000 * 4
#else
#use delay(clock=20000000,RESTART_WDT)
#endif
#ifdef PIC18LF6722
#use RS232(BAUD=115200, XMIT=PIN_G1,RCV=PIN_G2,BRGH1OK,RESTART_WDT,STREAM=UARTR1)
#else
#use RS232(BAUD=115200, XMIT=PIN_C6,RCV=PIN_C7,BRGH1OK,RESTART_WDT,STREAM=UARTR1)
#endif
//---------------------------------------------------------------------
//---------------------------------------------------------------------
//Constant Definitions
//---------------------------------------------------------------------
#define DIST 0 // Array index for segment distance
#define VEL 1 // Array index for segment vel. limit
#define ACCEL 2 // Array index for segment accell.
#define TIME 3 // Array index for segment delay time
#define INDEX RB0 // Input for encoder index pulse
#define NLIM RB1 // Input for negative limit switch
#define PLIM RB2 // Input for positive limit switch
#define input(PIN_B3) //GPI RB3 // General purpose input
#define MODE1 !input(PIN_B4) //PORTBbits.RB4 // DIP switch #1
#define MODE2 !input(PIN_B5) //PORTBbits.RB5 // DIP switch #2
#define MODE3 !input(PIN_B6) //PORTBbits.RB6 // DIP switch #3
#define MODE4 !input(PIN_B7) //PORTBbits.RB7 // DIP switch #4
#define SPULSE PIN_C5 //#define SPULSE RC5 // Software timing pulse output
#define ADRES ADRESH // Redefine for 10-bit A/D converter
#define KP 250
#define KI 252
#define KD 254
#define ADRESHL make16(ADRESH,ADRESL)
//---------------------------------------------------------------------
// Variable declarations
//---------------------------------------------------------------------
const char ready[] = "\r\nREADY>\r\n";
const char error[] = "\r\nERROR!\r\n";
signed int8 inpbuf[8]; //8bits
char
eeadrr,
firstseg,
lastseg,
segnum,
parameter,
i, // index to ASCII buffer
comcount, // index to input string
udata // received character from USART
;
//Un bit del byte stat
struct { // holds status bits for servo
unsigned phase:1; // first half/ second half of profile
unsigned neg_move:1; // backwards relative move
unsigned motion:1; //
unsigned saturated:1; // servo output is saturated
unsigned bit4:1;
unsigned bit5:1;
unsigned run:1;
unsigned loop:1;
} stat ;
union LNG{
signed int32 l;
unsigned int32 ul;
signed int16 i[2];
unsigned int16 ui[2];
unsigned int8 bb[4];
unsigned int8 ubb[4];
};
union LNG
accell,
temp,
u0,
ypid,
velact,
phase1dist
;
signed int16
dtime, // Segment delay time
integral,
kpp,
kii,
kdd, // PID gain constants
vlim, velcom,
mvelocity,
DnCount,
UpCount,
tempch;
signed int32
position, // Commanded position.
mposition, // Actual measured position.
fposition, // Originally commanded position.
flatcount; // Holds # of sample periods for which
// the velocity limit was reached in first
// half of the move.
float ValorADC;
signed int16 PosicionReal;
signed int16 Temporal1;
signed int16 OrdenPosOLD;
signed int16 OrdenPosNEW;
signed int16 OrdenPosADC;
signed int16 segment1[12][4]; // array in bank2 for 12 motion segments
signed int16 SegmentOrd[4]; // array in bank3 for 12 motion segments
unsigned char TramaRecUART[15]; //Trama recibida en UART de órdenes
unsigned int16 Timer0OLD,Timer1OLD; //Control de patinado del servo cuando llega a topes
char FLAG;
#bit ENBMotor = FLAG.0 // Indicador de que el motor se acaba de arrancar
#define ENBMotorASM FLAG,0
#bit OrdenRec = FLAG.1
#define OrdenRecASM FLAG,1
#ifdef PIC18LF6722
#define PINENBPW PIN_A5 //DIR en la versión anterior
#else
#define PINENBPW PIN_E2
#endif
#define EnableDriverPW() output_high(PINENBPW)
#define DisableDriverPW() output_low(PINENBPW)
#define TIMER3ON() bit_set(T3CON,TMR3ON);enable_interrupts(INT_TIMER3)
#define TIMER3OFF() bit_clear(T3CON,TMR3ON); disable_interrupts(INT_TIMER3)
#define TIMER2ON() bit_set(T2CON,TMR2ON)
#define TIMER2OFF() bit_clear(T2CON,TMR2ON)
//
// File: ZoomSubPID.C
//
/********************************************************************
Funciones del control PID
********************************************************************/
void UpdPos(void);
void UpdTraj(void);
void CalcError(void);
void CalcPID(void);
void SetupMove(void);
//---------------------------------------------------------------------
// UpdTraj()
// Computes the next required value for the next commanded motor
// position based on the current motion profile variables. Trapezoidal
// motion profiles are produced.
//---------------------------------------------------------------------
void UpdTraj(void){
if(stat.motion && !stat.saturated){
if(!stat.phase){ // If in the first half of the move.
if(velact.ui[1] < vlim) // If still below the velocity limit
velact.ul += accell.ul; // Accelerate
else // If velocity limit has been reached,
flatcount++; // increment flatcount.
temp.ul = velact.ul; // Put velocity value into temp
// and round to 16 bits
if(velact.ui[0] == 0x8000){
if(!(velact.ubb[2] & 0x01))
temp.ui[1]++;
else;
}
else
if(velact.ui[0] > 0x8000) temp.ui[1]++;
else;
phase1dist.ul -= (unsigned int32)temp.ui[1];
if(stat.neg_move)
position -= (unsigned int32)temp.ui[1];
else
position += (unsigned int32)temp.ui[1];
if(phase1dist.l <= 0) // If phase1dist is negative
// first half of the move has
stat.phase = 1; // completed.
}
else { // If in the second half of the move,
// Decrement flatcount if not 0.
if(flatcount) flatcount--;
else
if(velact.ul){ // If commanded velocity not 0,
velact.ul -= accell.ul; // Decelerate
if(velact.i[1] < 0)
velact.l = 0;
}
else{ // else
if(dtime) dtime--; // Decrement delay time if not 0.
else{
stat.motion = 0; // Move is done, clear motion flag
position = fposition;
}
}
temp.ul = velact.ul; // Put velocity value into temp
// and round to 16 bits
if(velact.ui[0] == 0x8000){
if(!(velact.ubb[2] & 0x01))
temp.ui[1]++;
else;
}
else
if(velact.ui[0] > 0x8000) temp.ui[1]++;
else;
if(stat.neg_move) // Update commanded position
position -= (unsigned int32)temp.ui[1];
else
position += (unsigned int32)temp.ui[1];
}//end else if(!stat.phase){
} // END if (stat.motion)
else{
if(stat.run && !stat.motion){ // If motion stopped and profile
// running, get next segment number
if(segnum < firstseg) segnum = firstseg;
if(segnum > lastseg){
segnum = firstseg; // Clear run flag if loop flag not set.
if(!stat.loop) stat.run = 0;
}
else{
SetupMove(); // Get data for next motion segment.
segnum++; // Increment segment number.
}
}
else;
}
}
//---------------------------------------------------------------------
// SetupMove()
// Gets data for next motion segment to be executed
//---------------------------------------------------------------------
void SetupMove(void){
/* if(segnum < 12){ // Get profile segment data from data memory.
phase1dist.i[0] = segment1[segnum][DIST];
vlim = segment1[segnum][VEL];
accell.i[0] = segment1[segnum][ACCEL];
dtime = segment1[segnum][TIME];
}*/
// if(segnum < 24){ //Solo el 23
phase1dist.i[0] = SegmentOrd[DIST];
vlim = SegmentOrd[VEL];
accell.i[0] = SegmentOrd[ACCEL];
dtime = SegmentOrd[TIME];
// }
phase1dist.bb[2] = phase1dist.bb[1]; // Rotate phase1dist one byte
phase1dist.bb[1] = phase1dist.bb[0]; // to the left.
phase1dist.bb[0] = 0;
if(phase1dist.bb[2] & 0x80) // Sign-extend value
phase1dist.bb[3] = 0xff;
else
phase1dist.bb[3] = 0;
accell.bb[3] = 0; // Rotate accell one byte to
accell.bb[2] = accell.bb[1]; // the left
accell.bb[1] = accell.bb[0];
accell.bb[0] = 0;
temp.l = position;
if(temp.ubb[0] > 0x7f) // A fractional value is left
temp.l += 0x100; // over in the 8 LSbits of
temp.ubb[0] = 0; // position, so round position
// variable to an integer value
position = temp.l; // before computing final move
// position.
fposition = position + phase1dist.l; // Compute final position for
// the move
if(phase1dist.bb[3] & 0x80){ // If the move is negative,
stat.neg_move = 1; // Set flag to indicate negative
phase1dist.l = -phase1dist.l; // move.
}
else stat.neg_move = 0; // Clear flag for positive move
phase1dist.l >>= 1; // phase1dist holds total
// move distance, so divide by 2
velact.l = 0; // Clear commanded velocity
flatcount = 0; // Clear flatcount
stat.phase = 0; // Clear flag:first half of move
if(accell.l && vlim)
stat.motion = 1; // Enable motion
}
//---------------------------------------------------------------------
// UpdPos()
//---------------------------------------------------------------------
void UpdPos(void){
// Old timer values are presently stored in UpCount and DnCount, so
// add them into result now.
mvelocity = DnCount;
mvelocity -= UpCount;
// Write new timer values into UpCount and DnCount variables.
UpCount = get_timer0(); //ReadTimer0();
DnCount = get_timer1(); //ReadTimer1();
// Add new count values into result.
mvelocity += UpCount;
mvelocity -= DnCount;
// Add measured velocity to measured position to get new motor
// measured position.
mposition += (signed int32)mvelocity << 8;
}
//---------------------------------------------------------------------
// CalcError()
// Calculates position error and limits to 16 bit result
//---------------------------------------------------------------------
void CalcError(void){
u0.l = mposition - position; // Get error
u0.bb[0] = u0.bb[1];
u0.bb[1] = u0.bb[2]; // shift to the right to discard
u0.bb[2] = u0.bb[3]; // lower 8 bits.
if (u0.bb[2] & 0x80){ // If error is negative.
u0.bb[3] = 0xff; // Sign-extend to 32 bits.
if((u0.i[1] != 0xffff) || !(u0.bb[1] & 0x80)){
u0.ui[1] = 0xffff; // Limit error to 16-bits.
u0.ui[0] = 0x8000;
}
else;
}
else{ // If error is positive.
u0.bb[3] = 0x00;
if((u0.i[1] != 0x0000) || (u0.bb[1] & 0x80)){
u0.ui[1] = 0x0000; // Limit error to 16-bits.
u0.ui[0] = 0x7fff;
}
else;
}
}
//---------------------------------------------------------------------
// CalcPID()
// Calculates PID compensator algorithm and determines new value for
// PWM duty cycle
//---------------------------------------------------------------------
void CalcPID(void){
ypid.l = (signed int32)u0.i[0]*(signed int32)kpp; // If proportional gain is not 0,
// calculate proportional term.
if(!stat.saturated) // If output is not saturated,
integral += u0.i[0]; // add present error to integral
// value.
if(kii) // If integral value is not 0,
ypid.l += (signed int32)integral*(signed int32)kii; // calculate integral term.
if(kdd) // If differential term is not 0,
ypid.l += (signed int32)mvelocity*(signed int32)kdd; // calculate differential term.
if(ypid.bb[3] & 0x80){ // If PID result is negative
if((ypid.bb[3] < 0xff) || !(ypid.bb[2] & 0x80)){
ypid.ui[1] = 0xff80; // Limit result to 24-bit value
ypid.ui[0] = 0x0000;
}
else;
}
else{ // If PID result is positive
if(ypid.bb[3] || (ypid.bb[2] > 0x7f)){
ypid.ui[1] = 0x007f; // Limit result to 24-bit value
ypid.ui[0] = 0xffff;
}
else;
}
ypid.bb[0] = ypid.bb[1]; // Shift PID result right to
ypid.bb[1] = ypid.bb[2]; // get upper 16 bits.
stat.saturated = 0; // Clear saturation flag and see
if(ypid.i[0] > 500){ // if present duty cycle output
// exceeds limits.
ypid.i[0] = 500;
stat.saturated = 1;
}
if(ypid.i[0] < -500){
ypid.i[0] = -500;
stat.saturated = 1;
}
ypid.i[0] += 512; // Add offset to get positive
ypid.i[0] <<= 6; // duty cycle and shift left to
// get 8 Msb's in upper byte.
CCPR1L = ypid.bb[1]; // Write upper byte to CCP register
// to set duty cycle.
// Set 2 LSb's of duty cycle in
// CCP1CON register.
if(ypid.bb[0] & 0x80) bit_set(CCP1CON,5); // CCP1X = 1;
else bit_clear(CCP1CON,5); //CCP1X = 0;
if(ypid.bb[0] & 0x40) bit_set(CCP1CON,4); //CCP1Y = 1;
else bit_clear(CCP1CON,4); //CCP1Y = 0;
}
//
// File: ZoomSUB.C
//
/********************************************************************
********************************************************************/
#define DisablePullups() bit_set(INTCON2,RBPU);
#define EnablePullups() bit_clear(INTCON2,RBPU);
// Function Prototypes-------------------------------------------------
void DoCommand(void);
void Setup(void);
void EEDatWrite(unsigned char address, int16 data);
signed int16 EEDatRead(unsigned char address);
void SetDCPWM1(unsigned int16 dutycycle);
void OpenPWM1( char period );
void EEDatWrite(unsigned char address, int16 data);
signed int16 EEDatRead(unsigned char address);
void DoCommand(void);
void CargaDistancia(signed int16 Distt);
void AnchoBanda(void);
//---------------------------------------------------------------------
// Setup() initializes program variables and peripheral registers
//---------------------------------------------------------------------
void Setup(void){
firstseg = 0; // 8 bits Initialize motion segment
lastseg = 0; // 8 bits variables
segnum = 0;
parameter = 0; // 8 bits Motion segment parameter#
i = 0; // Receive buffer index
comcount = 0; // Input command index
udata = 0; // Holds USART received data
//Un bit del byte stat
stat.phase = 0; // Initialize flags and variables
stat.saturated = 0; // used for servo calculations.
stat.motion = 0;
stat.run = 0;
stat.loop = 0;
stat.neg_move = 0;
dtime = 0;
integral = 0;
vlim = 0;
velcom = 0;
mvelocity = 0;
DnCount = 0;
UpCount = 0;
temp.l = 0;
accell.l = 0;
u0.l = 0;
ypid.l = 0;
velact.l = 0;
phase1dist.l = 0;
position = 0;
mposition = position;
fposition = position;
flatcount = 0;
memset(inpbuf,0,8); // clear the input buffer
udata = RCREG1;
udata = RCREG1;
RCREG1 = 0;
udata = 0;
#ifdef PLLMHz
T2CON = 0b11111000; //40MHz: INT cada 408us //Para el PWM solo actua el prescaler = 39,2 Khz
#else
T2CON = 0b11001000; // 19.53 Khz PWM @ 20MHz 0b11001000
#endif
bit_clear(PIR1,TMR2IF); //Borra INT
bit_set(PIE1,TMR2IE); //Habilita INT
bit_set(T2CON,TMR2ON); //Timer 2 ON
SetDCPWM1(512); // 50% initial duty cycle
OpenPWM1(0xff); // Setup Timer2, CCP1 to provide
EnablePullups(); // Enable PORTB pullups
T1CON = 0b10000010; //Enable 16 bits
bit_clear(PIE1,TMR1IE); //No INT
bit_clear(PIR1,TMR1IF); //Borra INT
set_timer1(0);
bit_set(T1CON,TMR1ON); //Timer1 ON
T0CON = 0b00101000; //16 bits
bit_clear(INTCON,TMR0IE); //No INT
TMR0L = 0; // Clear timers.
TMR0H = 0;
bit_set(T0CON,TMR0ON); //Timer0 ON
ADCON1 = 0b00001101; //AN0,AN1
ADCON0 = 0b00000001; //Canal 0
ADCON2 = 0b10111110; //Right,20TAD,Fosc/64
PORTC = 0; // Clear PORTC
PORTD = 0; // Clear PORTD
PORTE = 0x00; // Clear PORTE
TRISC = 0xdb; //
TRISD = 0; // PORTD all outputs
TRISE = 0; // PORTE all outputs
kpp = EEDatRead(KP); // Get PID gain values from
kii = 0; // data EEPROM
kdd = EEDatRead(KD); //
if(Kdd == 0xFFFF) Kdd= -1000; //Para el servo MG946R
if(Kpp == 0xFFFF) Kpp=6000; //Para el servo MG946R
//Timer3 para llevar a tope el zoom y que no patine
//set_timer3(53035); //53035 = 10ms. con 40MHz
set_timer3(400); //400 = 40ms. con 40MHz
setup_timer_3(T3_INTERNAL | T3_DIV_BY_8);
TIMER3OFF();
fprintf(UARTR1,"Configurado Servo...\r\n");
fprintf(UARTR1,ready);
enable_interrupts(GLOBAL);
#ifdef PIC18LF6722
enable_interrupts(INT_RDA2);
#else
enable_interrupts(INT_RDA);
#endif
}
/****************************************************************/
/* Union used to hold the 10-bit duty cycle */
union PWMDC{
unsigned int16 lpwm;
signed int8 bpwm[2]; //char
};
/****************************************************************/
/////////////////////////////////////////////
void SetDCPWM1(unsigned int16 dutycycle){
union PWMDC DCycle;
// Save the dutycycle value in the union
DCycle.lpwm = dutycycle << 6;
// Write the high byte into CCPR1L
CCPR1L = DCycle.bpwm[1];
// Write the low byte into CCP1CON5:4
CCP1CON = (CCP1CON & 0xCF) | ((DCycle.bpwm[0] >> 2) & 0x30);
}
/****************************************************************/
////////////////////////////////////
void OpenPWM1( char period ){
CCP1CON |= 0b00001100; //ccpxm3:ccpxm0 11xx=pwm mode
output_low(PIN_C2);
//---------------------------------------------------
bit_clear(T2CON,TMR2ON); // STOP TIMER2 registers to POR state
PR2 = period; // Set period
bit_set(T2CON,TMR2ON); // Turn on PWM1
}
/****************************************************************/
//---------------------------------------------------------------------
// ExtEEWrite()
// Writes an integer value to an EEPROM connected to the I2C bus at
// the specified location.
//---------------------------------------------------------------------
void EEDatWrite(unsigned char address, int16 data){
unsigned int Tmp;
Tmp = make8(data,0);
write_eeprom(address,Tmp); // Attempt to write low byte of data
Tmp = make8(data,1);
write_eeprom(address++,Tmp); // Attempt to write high byte of data
}
//---------------------------------------------------------------------
// ExtEEWrite()
// Reads an integer value from an EEPROM connected to the I2C bus at
// the specified location.
//---------------------------------------------------------------------
signed int16 EEDatRead(unsigned char address){
unsigned int8 TemH,TemL;
TemL = read_eeprom(address), // Attempt to read low byte of data
TemH = read_eeprom(address++); // Attempt to read high byte of data
return make16(TemH,TemL);
}
//-------------------------------------------------------------------
// DoCommand()
// Processes incoming USART data.
//-------------------------------------------------------------------
void DoCommand(void){
if(comcount == 0){ // If this is the first parameter of the input command...
switch(inpbuf[0]){
case 'X': parameter = DIST; // Segment distance change
break;
case 'V': parameter = VEL; // Segment velocity change
break;
case 'A': parameter = ACCEL; // Segment acceleration change
break;
case 'T': parameter = TIME; // Segment delay time change
break;
case 'P': parameter = 'P'; // Change proportional gain
break;
case 'I': parameter = 'I'; // Change integral gain
break;
case 'D': parameter = 'D'; // Change differential gain
break;
case 'L': parameter = 'L'; // Loop a range of segments
break;
case 'S': stat.run = 0; // Stop execution of segments
break;
case 'G': parameter = 'G'; // Execute a range of segments
break;
case 'W': if(ENBMotor){ // Enable or disable motor driver IC
DisableDriverPW();ENBMotor=False; //RE2 = 0;
//output_low(PIN_E2);ENBMotor=False; //RE2 = 0;
fprintf(UARTR1,"\r\nPWM Off\r\n");
}
else{
EnableDriverPW();ENBMotor=True;//RE2 = 1;
//output_high(PIN_E2);ENBMotor=True;//RE2 = 1;
fprintf(UARTR1,"\r\nPWM On\r\n");
}
break;
default: { if(inpbuf[0] != '\0'){
fprintf(UARTR1,"Error\r\n");
}
}
break;
}
}
else if(comcount == 1){ // If this is the second parameter of the input command.
if(parameter < 4) segnum = (char)(atol(inpbuf));
else
switch(parameter){
case 'P':{ kpp = atol(inpbuf); // proportional gain change
EEDatWrite(KP, kpp); // Store value in EEPROM
}
break;
case 'I':{ kii = atol(inpbuf); // integral gain change
EEDatWrite(KI, kii); // Store value in EEPROM
}
break;
case 'D':{ kdd = atol(inpbuf); // differential gain change
EEDatWrite(KD, kdd); // Store value in EEPROM
}
break;
case 'G': firstseg = (char)(atoi(inpbuf));
break;
// Get the first segment in
// the range to be executed.
case 'L': firstseg = (char)(atoi(inpbuf));
break;
default: break;
}
}
else if(comcount == 2){
////Al quitar if(!stat.run), cuando está en "loop" basta con enviar un nuevo "X"
//// para que actue. Con el if, hay que llevar a "Stop", cambiar el "X" y enviar "L"
//if(!stat.run){ // If no profile is executing
if(parameter < 4){ // If this was a segment parameter change.
/* if(segnum < 12){
// Write the segment paramater into data memory
segment1[segnum][parameter] = atol(inpbuf);
// Compute EEPROM address and write data to EEPROM
eeadrr = (segnum << 3) + (parameter << 1);
EEDatWrite(eeadrr, segment1[segnum][parameter]);
}*/
if(segnum < 24)
// Write segment parameter data into data memory
OrdenPosNEW = atol(inpbuf);
//SegmentOrd[parameter] = atol(inpbuf);
/*Temporal1 = OrdenPosNEW;
fprintf(UARTR1,"New: %ld\r\n",OrdenPosNEW);
if(OrdenPosNEW != OrdenPosOLD){
OrdenPosNEW -= OrdenPosOLD;
OrdenPosOLD = Temporal1;
SegmentOrd[parameter] = OrdenPosNEW; //atol(inpbuf);
}else{
SegmentOrd[parameter] = 0; //atol(inpbuf);
}
fprintf(UARTR1,"Sum: %ld\r\n",SegmentOrd[parameter]);*/
}
else switch(parameter){
case 'G':{ lastseg = (char)(atoi(inpbuf)); // Get value for
segnum = firstseg; // last segment.
stat.loop = 0;
stat.run = 1; // Start profile.
}
break;
case 'L':{ lastseg = (char)(atoi(inpbuf)); // Get value for
segnum = firstseg; // last segment.
stat.loop = 1; // Enable looping
stat.run = 1; // Start profile
}
break;
default: break;
}// else switch(parameter){
//}// if(!stat.run){
}// else if(comcount == 2){
else;
}
//-------------------------------------------------------------------
// ExtraeCommand()
// Processes incoming USART data.
//-------------------------------------------------------------------
void ExtraeCommand(char Caracter){
//if(bit_test(PIR1,RC1IF)){ // Check for USART interrupt
switch(Caracter){
case ',': DoCommand(); // process the string
memset(inpbuf,0,8); // clear the input buffer
i = 0; // clear the buffer index
comcount++; // increment comma count
break;
case 0x0d: DoCommand(); // process the string
memset(inpbuf,0,8); // clear the input buffer
i = 0; // clear the buffer index
comcount = 0; // clear comma count
segnum = 0; // clear segment number
parameter = 0; // clear paramater
break;
default: inpbuf[i] = Caracter, //udata; // get received char
i++; // increment buffer index
if(i > 7){ // If more than 8 chars received before getting
memset(inpbuf,0,8); // buffer
i = 0; // the buffer index
}
else; // {TXREG1 = udata; } //echo character
break;
}//end switch(udata)
}
//////////////////////////////////////
/////////////////////////////////////
void CargaDistancia(signed int16 Distt){
//segment2[segnum - 12][parameter] = atol(inpbuf);
SegmentOrd[DIST] = Distt; //Distancia
SegmentOrd[VEL] = 4096; //Velocidad
SegmentOrd[ACCEL] = 32000; //Aceleración
SegmentOrd[TIME] = 0; //Time
}
//////////////////////////////////////
/////////////////////////////////////
void AnchoBanda(void){
PORTD = 0; // Clear the LED bargraph display.
PORTE &= 0x04; // "
if(ADRES > 225){
PORTE |= 0x03; // Turn on 10 LEDS
PORTD = 0xff;
}
if(ADRES > 200){
PORTE |= 0x01; // Turn on 9 LEDS
PORTD = 0xff;
}
else if(ADRES > 175) PORTD = 0xff; // Turn on 8 LEDS
else if(ADRES > 150) PORTD = 0x7f; // 7 LEDS
else if(ADRES > 125) PORTD = 0x3f; // 6 LEDS
else if(ADRES > 100) PORTD = 0x1f; // 5 LEDS
else if(ADRES > 75) PORTD = 0x0f; // 4 LEDS
else if(ADRES > 50) PORTD = 0x07; // 3 LEDS
else if(ADRES > 25) PORTD = 0x03; // 2 LEDS
else if(ADRES > 0) PORTD = 0x01; // 1 LED
else;
}
//
// File: ZoomSYS.C
//
//---------------------------------------------------------------------
// servo_isr()
// Performs the servo calculations
//---------------------------------------------------------------------
///---------------------------------------------------------------------
// Interrupt Timer 2
// Esta interrupción sirve para que se ejecuten los controles
// cada xx ms.
// El temporizador se ajusta con el Prescaler y
// Postescaler del registro T2CON
// El PWM se ajusta solo con el Prescaler
//---------------------------------------------------------------------
#INT_TIMER2
void Timer2(){
output_high(SPULSE); // = 1; // Toggle output pin for ISR code timing
UpdTraj(); // Get new commanded position
UpdPos(); // Get new measured position
CalcError(); // Calculate new position errorr
CalcPID();
bit_clear(PIR1,TMR2IF); //PIR1bits.TMR2IF = 0; // Clear Timer2 Interrupt Flag.
output_low(SPULSE);// = 0; // Toggle output pin for ISR code timing
}
/////////////////////////////////////////////////////////////
// INT TIMER3
// La INT3 se emplea para llevar a tope el zoom y que no patine
// Con el servo MG946R en aceleración la distancia entre pulsos
// es de 12 ms máx y en frenado 22ms.
/////////////////////////////////////////////////////////////
#INT_TIMER3
void Timer3() {
set_timer3(400); //400 = 40ms. con 40MHz
if((get_timer0() == Timer0OLD) && (get_timer1() == Timer1OLD)){
TIMER3OFF();
}
else{
Timer0OLD=get_timer0();Timer1OLD = get_timer1();
}
}
/////////////////////////////////////////////////////////////
// INT RDA, RS232
// Recibe una orden através del la UART
/////////////////////////////////////////////////////////////
#ifdef PIC18LF6722
#INT_RDA2
#else
#INT_RDA
#endif
void UARTRx() {
unsigned char ByteRx,buffer;
unsigned int8 timeout;
ByteRx=fgetc(UARTR1);
#ifdef PIC18LF6722
bit_clear(PIR1,RC1IF); //Borra bandera de INT
#else
bit_clear(PIR1,RCIF);
#endif
TramaRecUART[buffer]=ByteRx;
buffer++; // Incrementa string pointer, el primero es ">"
comcount = 0;
do{
timeout =0; //Espera a recibir, y si no, solo espera 1ms después de recibir el último
while (!kbhit(UARTR1) && (++timeout<100))delay_us(10);
if(kbhit(UARTR1)) {
ByteRx = fgetc(UARTR1); } // Get un caracter de la USART
else{OrdenRec = False;break;}
TramaRecUART[buffer] = ByteRx;
buffer++; // Incrementa string pointer
//if(ByteRx == ',') comcount++; //N parametros
if(buffer >80){buffer = 0; break;}
}while(ByteRx > 9);//9 //Incluye CR y LF
OrdenRec = True;
if((ByteRx == 13)) { //|| (ByteRx == 10)){ //Incluye CR y LF
TramaRecUART[buffer] = 0;
OrdenRec = True;
}
}
//
// File: ZoomAN696.C
//
/////////////////////////////////////////////////////////////////////////////
// CONTROL DE MOTORRES PARA PLATAFORMA ///
// GIROESTABILIZADA ///
// PIC "C" Compiler PCM ///
// Esta versión es la que procede de la original del PIC18C452 ///
// Se ha añadido la comprobación, después de leer la EEPROM, ///
// si Kp y Kd son 0xFFFF. Esto indica que la EEPROM estaba ///
// borrada. Un valor 0xFFFF hace que no funcione nada ///
// Se inicializa Kp con 6000 y Kd con -1000 (0 a 6000 y -32000 a 32000)///
/// para el servo MG946R ///
///////////////////////////////////////////////////////////////////////////////
//#define PIC18LF6722
#define PLLMHz
#define Debuger1
#ifdef PIC18LF6722
#include <18F6722.h>
#include <p18f6722m.inc>
#else
#include <18F8722.h>
#include <p18f8722m.inc>
#endif
#device *=16
#include "STRING.H"
#include "STDLIB.H"
#include <ZoomAN696.h>
#include <ZoomSubPID.c>
#include <ZoomSUB.c>
#include <ZoomSYS.c>
#ZERO_RAM
//#priority ext, timer1,timer3
//---------------------------------------------------------------------
// main()
//---------------------------------------------------------------------
void main(){
unsigned int8 CaracterRec,NCaracterRec;
fprintf(UARTR1,"Configurando PIC18xx...\r\n");
Setup();
fprintf(UARTR1,"PWM On\r\n");
EnableDriverPW();ENBMotor=True; //RE2 = 1;
SegmentOrd[DIST] = 450;
//CargaDistancia(450);
SegmentOrd[VEL] = 20; //10 4096Velocidad
//SegmentOrd[ACCEL] = 32000; //Aceleración
SegmentOrd[ACCEL] = 50; //6000 Acelera
SegmentOrd[TIME] = 0; //Time
firstseg =23;lastseg=23;
stat.loop = 0;
stat.run = 1;
delay_ms(100); //Espera que arranque
TIMER3ON();
do{
delay_us(100);
}while(bit_test(T3CON,TMR3ON));
//stat.run = 0;
// set_timer0(450);
SegmentOrd[DIST] = 0;
mposition = position =fposition;
stat.motion = flatcount = velact = dtime = 0; stat.motion = 0;
fprintf(UARTR1,"mpos: %Ld fpos: %Ld posi: %Ld\r\n",mposition,fposition,position);
fprintf(UARTR1,"T0: %Ld T1: %Ld\r\n",get_timer0(),get_timer1());
delay_ms(100); //Espera que arranque
set_timer1(0);
set_timer0(0);
UpCount = DnCount = 0;
SegmentOrd[DIST] = -450;
//CargaDistancia(-450);
firstseg =23;lastseg=23;
stat.loop = 0;
stat.run = 1;
delay_ms(100); //Espera que arranque
TIMER3ON();
do{
delay_us(100);
}while(bit_test(T3CON,TMR3ON));
PosicionReal = get_timer0() - get_timer1();
// set_timer1(450);
SegmentOrd[DIST] = 0;
mposition = position =fposition;
stat.motion = flatcount = velact = dtime = 0; stat.motion = 0;
delay_ms(50); //250
set_timer1(0); set_timer0(0); PosicionReal = 0;
DnCount = UpCount = flatcount = velact = dtime = 0; stat.motion = 0;
OrdenPosOLD = OrdenPosNEW = SegmentOrd[DIST] = 0;
////////////////////////////////////////////////////
///////////////////////////////////////////////////
while(1){
restart_wdt();
bit_clear(PIR1,ADIF); //Borra INT
bit_set(ADCON0,GO); // Start an A/D conversion
while(!bit_test(PIR1,ADIF)); //while(ADGO); // Wait for the conversion to complete
ValorADC = (float)ADRESHL;
ValorADC = (ValorADC*100)/1024;
OrdenPosADC = (unsigned int16)ValorADC;
if(ValorADC >60) PORTD = 0b10000000;
else if(ValorADC >55) PORTD = 0b01000000;
else if(ValorADC >50) PORTD = 0b00100000;
else if(ValorADC >47) PORTD = 0b00010000;
else if(ValorADC >45) PORTD = 0b00001000;
else if(ValorADC >40) PORTD = 0b00000100;
else if(ValorADC >35) PORTD = 0b00000010;
else if(ValorADC >30) PORTD = 0b00000001;
else;
// fprintf(UARTR1,"New: %ld, %ld\r\n",OrdenPosNEW,PosicionReal);
if(OrdenRec){
NCaracterRec = strlen(TramaRecUART);
for(CaracterRec=0;CaracterRec<NCaracterRec;CaracterRec++){
ExtraeCommand(TramaRecUART[CaracterRec]);
}
OrdenRec = False;
}
}//end while(1)
}
|
|
|
|
Jerry I
Joined: 14 Sep 2003
Posts: 96
Location: Toronto, Ontario, Canada
|
|
| Posted: Tue Dec 10, 2013 10:58 pm |
|
|
Hi
I did a conversion of this AN 2 years ago, but did get to work but never actually used it.
The function your looking for to go directly to an encoder position, I did implement. Your code is different than mine, but I did try to add the functionality to your code.
I add new variable travel_to.
and new command R
changed T for Time -> H Hold Time
made T for Travel to position.
Look though the changes I added to your code should find.
/***** ADDED NEW xxxxxxxxxxx
I did not compile your code, so I am not sure if something is missing. I just compared my code to yours and added the changes I had made.
Not sure it will work. It took me a good month to get my code working.
Let me know.
| Code: |
//
//
File: ZoomAN696.H
//
///////////////////////////////////////////////////////////////////////////////
//////// Program memory: 0x16 Data RAM: 3840 Stack: 31
//////// I/O: 40 Analog Pins: 12
//////// Data EEPROM: 1024
//////// C Scratch area: 00 ID Location: 2000
//////// Fuses: LP,XT,HS,RC,EC,EC_IO,H4,RC_IO,PROTECT,NOPROTECT,IESO,NOIESO
//////// Fuses: NOBROWNOUT,BROWNOUT,WDT1,WDT2,WDT4,WDT8,WDT16,WDT32,WDT64
//////// Fuses: WDT128,WDT,NOWDT,BORV25,BORV27,BORV42,BORV45,PUT,NOPUT,CPD
//////// Fuses: NOCPD,NOSTVREN,STVREN,NODEBUG,DEBUG,NOLVP,LVP,WRT,NOWRT,CPB
//////// Fuses: NOCPB,EBTRB,NOEBTRB,EBTR,NOEBTR,CCP2B3,CCP2C1,WRTD,NOWRTD
//////// Fuses: WRTC,NOWRTC,WRTB,NOWRTB,FCMEN,NOFCMEN,INTRC,INTRC_IO
//////// Fuses: BROWNOUT_SW,BROWNOUT_NOSL,WDT_256,WDT_512,WDT_1024,WDT_2048
//////// Fuses: WDT_4096,WDT_8192,WDT_16384,WDT_32768,LPT1OSC,NOLPT1OSC,MCLR
//////// Fuses: NOMCLR,XINST,NOXINST,BBSIZE1K,BBSIZE2K,BBSIZE4K
///////////////////////////////////////////////////////////////////////////////
#ifdef Debuger1
#ifdef PLLMHz
#fuses H4,noWDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP,DEBUG, WDT128,NOEBTR,BROWNOUT
#else
#fuses HS,noWDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP,DEBUG, WDT128,NOEBTR,BROWNOUT
#endif
#else
#ifdef PLLMHz
#fuses H4,WDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP, noDEBUG, WDT128,NOEBTR,BROWNOUT
#else
#fuses HS,WDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP, noDEBUG, WDT128,NOEBTR,BROWNOUT
#endif
#endif
#ifdef PLLMHz
#use delay(clock=40000000,RESTART_WDT) //10.000.000 * 4
#else
#use delay(clock=20000000,RESTART_WDT)
#endif
#ifdef PIC18LF6722
#use RS232(BAUD=115200, XMIT=PIN_G1,RCV=PIN_G2,BRGH1OK,RESTART_WDT,STREAM=UARTR1)
#else
#use RS232(BAUD=115200, XMIT=PIN_C6,RCV=PIN_C7,BRGH1OK,RESTART_WDT,STREAM=UARTR1)
#endif
//---------------------------------------------------------------------
//---------------------------------------------------------------------
//Constant Definitions
//---------------------------------------------------------------------
#define DIST 0 // Array index for segment distance
#define VEL 1 // Array index for segment vel. limit
#define ACCEL 2 // Array index for segment accell.
#define TIME 3 // Array index for segment delay time
/***********ADDED NEW DEFINE **************/
#define TRAVEL 4 // Array indev for segment motor travel via encoder
/***********END NEW CODE*********/
#define INDEX RB0 // Input for encoder index pulse
#define NLIM RB1 // Input for negative limit switch
#define PLIM RB2 // Input for positive limit switch
#define input(PIN_B3) //GPI RB3 // General purpose input
#define MODE1 !input(PIN_B4) //PORTBbits.RB4 // DIP switch #1
#define MODE2 !input(PIN_B5) //PORTBbits.RB5 // DIP switch #2
#define MODE3 !input(PIN_B6) //PORTBbits.RB6 // DIP switch #3
#define MODE4 !input(PIN_B7) //PORTBbits.RB7 // DIP switch #4
#define SPULSE PIN_C5 //#define SPULSE RC5 // Software timing pulse output
#define ADRES ADRESH // Redefine for 10-bit A/D converter
#define KP 250
#define KI 252
#define KD 254
#define ADRESHL make16(ADRESH,ADRESL)
//---------------------------------------------------------------------
// Variable declarations
//---------------------------------------------------------------------
const char ready[] = "\r\nREADY>\r\n";
const char error[] = "\r\nERROR!\r\n";
signed int8 inpbuf[8]; //8bits
char
eeadrr,
firstseg,
lastseg,
segnum,
parameter,
i, // index to ASCII buffer
comcount, // index to input string
udata // received character from USART
;
//Un bit del byte stat
struct { // holds status bits for servo
unsigned phase:1; // first half/ second half of profile
unsigned neg_move:1; // backwards relative move
unsigned motion:1; //
unsigned saturated:1; // servo output is saturated
unsigned bit4:1;
unsigned bit5:1;
unsigned run:1;
unsigned loop:1;
} stat ;
union LNG{
signed int32 l;
unsigned int32 ul;
signed int16 i[2];
unsigned int16 ui[2];
unsigned int8 bb[4];
unsigned int8 ubb[4];
};
union LNG
accell,
temp,
u0,
ypid,
velact,
phase1dist
;
signed int16
dtime, // Segment delay time
integral,
kpp,
kii,
kdd, // PID gain constants
vlim, velcom,
mvelocity,
DnCount,
UpCount,
tempch;
signed int32
/***********ADDED NEW VARIABLE **************/
travel_to, // Travel_to Position
/***********END NEW VARIABLE **************/
position, // Commanded position.
mposition, // Actual measured position.
fposition, // Originally commanded position.
flatcount; // Holds # of sample periods for which
// the velocity limit was reached in first
// half of the move.
float ValorADC;
signed int16 PosicionReal;
signed int16 Temporal1;
signed int16 OrdenPosOLD;
signed int16 OrdenPosNEW;
signed int16 OrdenPosADC;
signed int16 segment1[12][4]; // array in bank2 for 12 motion segments
signed int16 SegmentOrd[4]; // array in bank3 for 12 motion segments
unsigned char TramaRecUART[15]; //Trama recibida en UART de órdenes
unsigned int16 Timer0OLD,Timer1OLD; //Control de patinado del servo cuando llega a topes
char FLAG;
#bit ENBMotor = FLAG.0 // Indicador de que el motor se acaba de arrancar
#define ENBMotorASM FLAG,0
#bit OrdenRec = FLAG.1
#define OrdenRecASM FLAG,1
#ifdef PIC18LF6722
#define PINENBPW PIN_A5 //DIR en la versión anterior
#else
#define PINENBPW PIN_E2
#endif
#define EnableDriverPW() output_high(PINENBPW)
#define DisableDriverPW() output_low(PINENBPW)
#define TIMER3ON() bit_set(T3CON,TMR3ON);enable_interrupts(INT_TIMER3)
#define TIMER3OFF() bit_clear(T3CON,TMR3ON); disable_interrupts(INT_TIMER3)
#define TIMER2ON() bit_set(T2CON,TMR2ON)
#define TIMER2OFF() bit_clear(T2CON,TMR2ON)
//
// File: ZoomSubPID.C
//
/********************************************************************
Funciones del control PID
********************************************************************/
void UpdPos(void);
void UpdTraj(void);
void CalcError(void);
void CalcPID(void);
void SetupMove(void);
//---------------------------------------------------------------------
// UpdTraj()
// Computes the next required value for the next commanded motor
// position based on the current motion profile variables. Trapezoidal
// motion profiles are produced.
//---------------------------------------------------------------------
void UpdTraj(void){
if(stat.motion && !stat.saturated){
if(!stat.phase){ // If in the first half of the move.
if(velact.ui[1] < vlim) // If still below the velocity limit
velact.ul += accell.ul; // Accelerate
else // If velocity limit has been reached,
flatcount++; // increment flatcount.
temp.ul = velact.ul; // Put velocity value into temp
// and round to 16 bits
if(velact.ui[0] == 0x8000){
if(!(velact.ubb[2] & 0x01))
temp.ui[1]++;
else;
}
else
if(velact.ui[0] > 0x8000) temp.ui[1]++;
else;
phase1dist.ul -= (unsigned int32)temp.ui[1];
if(stat.neg_move)
position -= (unsigned int32)temp.ui[1];
else
position += (unsigned int32)temp.ui[1];
if(phase1dist.l <= 0) // If phase1dist is negative
// first half of the move has
stat.phase = 1; // completed.
}
else { // If in the second half of the move,
// Decrement flatcount if not 0.
if(flatcount) flatcount--;
else
if(velact.ul){ // If commanded velocity not 0,
velact.ul -= accell.ul; // Decelerate
if(velact.i[1] < 0)
velact.l = 0;
}
else{ // else
if(dtime) dtime--; // Decrement delay time if not 0.
else{
stat.motion = 0; // Move is done, clear motion flag
position = fposition;
}
}
temp.ul = velact.ul; // Put velocity value into temp
// and round to 16 bits
if(velact.ui[0] == 0x8000){
if(!(velact.ubb[2] & 0x01))
temp.ui[1]++;
else;
}
else
if(velact.ui[0] > 0x8000) temp.ui[1]++;
else;
if(stat.neg_move) // Update commanded position
position -= (unsigned int32)temp.ui[1];
else
position += (unsigned int32)temp.ui[1];
}//end else if(!stat.phase){
} // END if (stat.motion)
else{
if(stat.run && !stat.motion){ // If motion stopped and profile
// running, get next segment number
if(segnum < firstseg) segnum = firstseg;
if(segnum > lastseg){
segnum = firstseg; // Clear run flag if loop flag not set.
if(!stat.loop) stat.run = 0;
}
else{
SetupMove(); // Get data for next motion segment.
segnum++; // Increment segment number.
}
}
else;
}
}
//---------------------------------------------------------------------
// SetupMove()
// Gets data for next motion segment to be executed
//---------------------------------------------------------------------
void SetupMove(void){
/* if(segnum < 12){ // Get profile segment data from data memory.
phase1dist.i[0] = segment1[segnum][DIST];
vlim = segment1[segnum][VEL];
accell.i[0] = segment1[segnum][ACCEL];
dtime = segment1[segnum][TIME];
}*/
// if(segnum < 24){ //Solo el 23
phase1dist.i[0] = SegmentOrd[DIST];
vlim = SegmentOrd[VEL];
accell.i[0] = SegmentOrd[ACCEL];
dtime = SegmentOrd[TIME];
/***********ADDED NEW CODE**************/
travel_to = SegmentOrd[TRAVEL]
/***********END NEW CODE*********/
// }
phase1dist.bb[2] = phase1dist.bb[1]; // Rotate phase1dist one byte
phase1dist.bb[1] = phase1dist.bb[0]; // to the left.
phase1dist.bb[0] = 0;
if(phase1dist.bb[2] & 0x80) // Sign-extend value
phase1dist.bb[3] = 0xff;
else
phase1dist.bb[3] = 0;
accell.bb[3] = 0; // Rotate accell one byte to
accell.bb[2] = accell.bb[1]; // the left
accell.bb[1] = accell.bb[0];
accell.bb[0] = 0;
temp.l = position;
if(temp.ubb[0] > 0x7f) // A fractional value is left
temp.l += 0x100; // over in the 8 LSbits of
temp.ubb[0] = 0; // position, so round position
// variable to an integer value
position = temp.l; // before computing final move
// position.
fposition = position + phase1dist.l; // Compute final position for
// the move
if(phase1dist.bb[3] & 0x80){ // If the move is negative,
stat.neg_move = 1; // Set flag to indicate negative
phase1dist.l = -phase1dist.l; // move.
}
else stat.neg_move = 0; // Clear flag for positive move
phase1dist.l >>= 1; // phase1dist holds total
// move distance, so divide by 2
velact.l = 0; // Clear commanded velocity
flatcount = 0; // Clear flatcount
stat.phase = 0; // Clear flag:first half of move
if(accell.l && vlim)
stat.motion = 1; // Enable motion
}
//---------------------------------------------------------------------
// UpdPos()
//---------------------------------------------------------------------
void UpdPos(void){
// Old timer values are presently stored in UpCount and DnCount, so
// add them into result now.
mvelocity = DnCount;
mvelocity -= UpCount;
// Write new timer values into UpCount and DnCount variables.
UpCount = get_timer0(); //ReadTimer0();
DnCount = get_timer1(); //ReadTimer1();
// Add new count values into result.
mvelocity += UpCount;
mvelocity -= DnCount;
// Add measured velocity to measured position to get new motor
// measured position.
mposition += (signed int32)mvelocity << 8;
}
//---------------------------------------------------------------------
// CalcError()
// Calculates position error and limits to 16 bit result
//---------------------------------------------------------------------
void CalcError(void){
u0.l = mposition - position; // Get error
u0.bb[0] = u0.bb[1];
u0.bb[1] = u0.bb[2]; // shift to the right to discard
u0.bb[2] = u0.bb[3]; // lower 8 bits.
if (u0.bb[2] & 0x80){ // If error is negative.
u0.bb[3] = 0xff; // Sign-extend to 32 bits.
if((u0.i[1] != 0xffff) || !(u0.bb[1] & 0x80)){
u0.ui[1] = 0xffff; // Limit error to 16-bits.
u0.ui[0] = 0x8000;
}
else;
}
else{ // If error is positive.
u0.bb[3] = 0x00;
if((u0.i[1] != 0x0000) || (u0.bb[1] & 0x80)){
u0.ui[1] = 0x0000; // Limit error to 16-bits.
u0.ui[0] = 0x7fff;
}
else;
}
}
//---------------------------------------------------------------------
// CalcPID()
// Calculates PID compensator algorithm and determines new value for
// PWM duty cycle
//---------------------------------------------------------------------
void CalcPID(void){
ypid.l = (signed int32)u0.i[0]*(signed int32)kpp; // If proportional gain is not 0,
// calculate proportional term.
if(!stat.saturated) // If output is not saturated,
integral += u0.i[0]; // add present error to integral
// value.
if(kii) // If integral value is not 0,
ypid.l += (signed int32)integral*(signed int32)kii; // calculate integral term.
if(kdd) // If differential term is not 0,
ypid.l += (signed int32)mvelocity*(signed int32)kdd; // calculate differential term.
if(ypid.bb[3] & 0x80){ // If PID result is negative
if((ypid.bb[3] < 0xff) || !(ypid.bb[2] & 0x80)){
ypid.ui[1] = 0xff80; // Limit result to 24-bit value
ypid.ui[0] = 0x0000;
}
else;
}
else{ // If PID result is positive
if(ypid.bb[3] || (ypid.bb[2] > 0x7f)){
ypid.ui[1] = 0x007f; // Limit result to 24-bit value
ypid.ui[0] = 0xffff;
}
else;
}
ypid.bb[0] = ypid.bb[1]; // Shift PID result right to
ypid.bb[1] = ypid.bb[2]; // get upper 16 bits.
stat.saturated = 0; // Clear saturation flag and see
if(ypid.i[0] > 500){ // if present duty cycle output
// exceeds limits.
ypid.i[0] = 500;
stat.saturated = 1;
}
if(ypid.i[0] < -500){
ypid.i[0] = -500;
stat.saturated = 1;
}
ypid.i[0] += 512; // Add offset to get positive
ypid.i[0] <<= 6; // duty cycle and shift left to
// get 8 Msb's in upper byte.
CCPR1L = ypid.bb[1]; // Write upper byte to CCP register
// to set duty cycle.
// Set 2 LSb's of duty cycle in
// CCP1CON register.
if(ypid.bb[0] & 0x80) bit_set(CCP1CON,5); // CCP1X = 1;
else bit_clear(CCP1CON,5); //CCP1X = 0;
if(ypid.bb[0] & 0x40) bit_set(CCP1CON,4); //CCP1Y = 1;
else bit_clear(CCP1CON,4); //CCP1Y = 0;
}
//
// File: ZoomSUB.C
//
/********************************************************************
********************************************************************/
#define DisablePullups() bit_set(INTCON2,RBPU);
#define EnablePullups() bit_clear(INTCON2,RBPU);
// Function Prototypes-------------------------------------------------
void DoCommand(void);
void Setup(void);
void EEDatWrite(unsigned char address, int16 data);
signed int16 EEDatRead(unsigned char address);
void SetDCPWM1(unsigned int16 dutycycle);
void OpenPWM1( char period );
void EEDatWrite(unsigned char address, int16 data);
signed int16 EEDatRead(unsigned char address);
void DoCommand(void);
void CargaDistancia(signed int16 Distt);
void AnchoBanda(void);
//---------------------------------------------------------------------
// Setup() initializes program variables and peripheral registers
//---------------------------------------------------------------------
void Setup(void){
firstseg = 0; // 8 bits Initialize motion segment
lastseg = 0; // 8 bits variables
segnum = 0;
parameter = 0; // 8 bits Motion segment parameter#
i = 0; // Receive buffer index
comcount = 0; // Input command index
udata = 0; // Holds USART received data
//Un bit del byte stat
stat.phase = 0; // Initialize flags and variables
stat.saturated = 0; // used for servo calculations.
stat.motion = 0;
stat.run = 0;
stat.loop = 0;
stat.neg_move = 0;
dtime = 0;
integral = 0;
vlim = 0;
velcom = 0;
mvelocity = 0;
DnCount = 0;
UpCount = 0;
temp.l = 0;
accell.l = 0;
u0.l = 0;
ypid.l = 0;
velact.l = 0;
phase1dist.l = 0;
position = 0;
mposition = position;
fposition = position;
flatcount = 0;
/***********ADDED NEW CODE VARIABLE**************/
travel_to = 0;
/***********END NEW CODE*********/
memset(inpbuf,0,8); // clear the input buffer
udata = RCREG1;
udata = RCREG1;
RCREG1 = 0;
udata = 0;
#ifdef PLLMHz
T2CON = 0b11111000; //40MHz: INT cada 408us //Para el PWM solo actua el prescaler = 39,2 Khz
#else
T2CON = 0b11001000; // 19.53 Khz PWM @ 20MHz 0b11001000
#endif
bit_clear(PIR1,TMR2IF); //Borra INT
bit_set(PIE1,TMR2IE); //Habilita INT
bit_set(T2CON,TMR2ON); //Timer 2 ON
SetDCPWM1(512); // 50% initial duty cycle
OpenPWM1(0xff); // Setup Timer2, CCP1 to provide
EnablePullups(); // Enable PORTB pullups
T1CON = 0b10000010; //Enable 16 bits
bit_clear(PIE1,TMR1IE); //No INT
bit_clear(PIR1,TMR1IF); //Borra INT
set_timer1(0);
bit_set(T1CON,TMR1ON); //Timer1 ON
T0CON = 0b00101000; //16 bits
bit_clear(INTCON,TMR0IE); //No INT
TMR0L = 0; // Clear timers.
TMR0H = 0;
bit_set(T0CON,TMR0ON); //Timer0 ON
ADCON1 = 0b00001101; //AN0,AN1
ADCON0 = 0b00000001; //Canal 0
ADCON2 = 0b10111110; //Right,20TAD,Fosc/64
PORTC = 0; // Clear PORTC
PORTD = 0; // Clear PORTD
PORTE = 0x00; // Clear PORTE
TRISC = 0xdb; //
TRISD = 0; // PORTD all outputs
TRISE = 0; // PORTE all outputs
kpp = EEDatRead(KP); // Get PID gain values from
kii = 0; // data EEPROM
kdd = EEDatRead(KD); //
if(Kdd == 0xFFFF) Kdd= -1000; //Para el servo MG946R
if(Kpp == 0xFFFF) Kpp=6000; //Para el servo MG946R
//Timer3 para llevar a tope el zoom y que no patine
//set_timer3(53035); //53035 = 10ms. con 40MHz
set_timer3(400); //400 = 40ms. con 40MHz
setup_timer_3(T3_INTERNAL | T3_DIV_BY_8);
TIMER3OFF();
fprintf(UARTR1,"Configurado Servo...\r\n");
fprintf(UARTR1,ready);
enable_interrupts(GLOBAL);
#ifdef PIC18LF6722
enable_interrupts(INT_RDA2);
#else
enable_interrupts(INT_RDA);
#endif
}
/****************************************************************/
/* Union used to hold the 10-bit duty cycle */
union PWMDC{
unsigned int16 lpwm;
signed int8 bpwm[2]; //char
};
/****************************************************************/
/////////////////////////////////////////////
void SetDCPWM1(unsigned int16 dutycycle){
union PWMDC DCycle;
// Save the dutycycle value in the union
DCycle.lpwm = dutycycle << 6;
// Write the high byte into CCPR1L
CCPR1L = DCycle.bpwm[1];
// Write the low byte into CCP1CON5:4
CCP1CON = (CCP1CON & 0xCF) | ((DCycle.bpwm[0] >> 2) & 0x30);
}
/****************************************************************/
////////////////////////////////////
void OpenPWM1( char period ){
CCP1CON |= 0b00001100; //ccpxm3:ccpxm0 11xx=pwm mode
output_low(PIN_C2);
//---------------------------------------------------
bit_clear(T2CON,TMR2ON); // STOP TIMER2 registers to POR state
PR2 = period; // Set period
bit_set(T2CON,TMR2ON); // Turn on PWM1
}
/****************************************************************/
//---------------------------------------------------------------------
// ExtEEWrite()
// Writes an integer value to an EEPROM connected to the I2C bus at
// the specified location.
//---------------------------------------------------------------------
void EEDatWrite(unsigned char address, int16 data){
unsigned int Tmp;
Tmp = make8(data,0);
write_eeprom(address,Tmp); // Attempt to write low byte of data
Tmp = make8(data,1);
write_eeprom(address++,Tmp); // Attempt to write high byte of data
}
//---------------------------------------------------------------------
// ExtEEWrite()
// Reads an integer value from an EEPROM connected to the I2C bus at
// the specified location.
//---------------------------------------------------------------------
signed int16 EEDatRead(unsigned char address){
unsigned int8 TemH,TemL;
TemL = read_eeprom(address), // Attempt to read low byte of data
TemH = read_eeprom(address++); // Attempt to read high byte of data
return make16(TemH,TemL);
}
//-------------------------------------------------------------------
// DoCommand()
// Processes incoming USART data.
//-------------------------------------------------------------------
void DoCommand(void){
if(comcount == 0){ // If this is the first parameter of the input command...
switch(inpbuf[0]){
case 'X': parameter = DIST; // Segment distance change
break;
case 'V': parameter = VEL; // Segment velocity change
break;
case 'A': parameter = ACCEL; // Segment acceleration change
break;
/***********ADDED NEW CODE CHANGED COMMAND T TIME to H for HOLD TIME **************/
case 'H': parameter = TIME; // Segment HOLD delay time change
break;
// case 'T': parameter = TIME; // Segment delay time change
// break;
/***********END NEW CODE CHANGED **************/
/***********ADDED NEW CODE MADE COMMAND T -> TRAVEL **************/
case 'T': parameter = TRAVEL; // Segment encoder travel to point change
break;
/***********END NEW CODE CHANGED **************/
case 'P': parameter = 'P'; // Change proportional gain
break;
case 'I': parameter = 'I'; // Change integral gain
break;
case 'D': parameter = 'D'; // Change differential gain
break;
case 'L': parameter = 'L'; // Loop a range of segments
break;
case 'S': stat.run = 0; // Stop execution of segments
break;
case 'G': parameter = 'G'; // Execute a range of segments
break;
/***********ADDED NEW CODE MADE COMMAND R -> RUN to **************/
case 'R': parameter = 'R'; // Run segments
break;
/*********** END NEW CODE COMMAND R Run **************/
case 'W': if(ENBMotor){ // Enable or disable motor driver IC
DisableDriverPW();ENBMotor=False; //RE2 = 0;
//output_low(PIN_E2);ENBMotor=False; //RE2 = 0;
fprintf(UARTR1,"\r\nPWM Off\r\n");
}
else{
EnableDriverPW();ENBMotor=True;//RE2 = 1;
//output_high(PIN_E2);ENBMotor=True;//RE2 = 1;
fprintf(UARTR1,"\r\nPWM On\r\n");
}
break;
default: { if(inpbuf[0] != '\0'){
fprintf(UARTR1,"Error\r\n");
}
}
break;
}
}
else if(comcount == 1){ // If this is the second parameter of the input command.
if(parameter < 4) segnum = (char)(atol(inpbuf));
else
switch(parameter){
case 'P':{ kpp = atol(inpbuf); // proportional gain change
EEDatWrite(KP, kpp); // Store value in EEPROM
}
break;
case 'I':{ kii = atol(inpbuf); // integral gain change
EEDatWrite(KI, kii); // Store value in EEPROM
}
break;
case 'D':{ kdd = atol(inpbuf); // differential gain change
EEDatWrite(KD, kdd); // Store value in EEPROM
}
break;
case 'G': firstseg = (char)(atoi(inpbuf));
break;
// Get the first segment in
// the range to be executed.
/***********ADDED NEW CODE COMMAND R Run **************/
case 'R':
firstseg = (char) (atoi(inpbuf));
break;
/*********** END NEW CODE COMMAND R Run **************/
case 'L': firstseg = (char)(atoi(inpbuf));
break;
default: break;
}
}
else if(comcount == 2){
////Al quitar if(!stat.run), cuando está en "loop" basta con enviar un nuevo "X"
//// para que actue. Con el if, hay que llevar a "Stop", cambiar el "X" y enviar "L"
//if(!stat.run){ // If no profile is executing
if(parameter < 4){ // If this was a segment parameter change.
/* if(segnum < 12){
// Write the segment paramater into data memory
segment1[segnum][parameter] = atol(inpbuf);
// Compute EEPROM address and write data to EEPROM
eeadrr = (segnum << 3) + (parameter << 1);
EEDatWrite(eeadrr, segment1[segnum][parameter]);
}*/
if(segnum < 24)
// Write segment parameter data into data memory
OrdenPosNEW = atol(inpbuf);
//SegmentOrd[parameter] = atol(inpbuf);
/*Temporal1 = OrdenPosNEW;
fprintf(UARTR1,"New: %ld\r\n",OrdenPosNEW);
if(OrdenPosNEW != OrdenPosOLD){
OrdenPosNEW -= OrdenPosOLD;
OrdenPosOLD = Temporal1;
SegmentOrd[parameter] = OrdenPosNEW; //atol(inpbuf);
}else{
SegmentOrd[parameter] = 0; //atol(inpbuf);
}
fprintf(UARTR1,"Sum: %ld\r\n",SegmentOrd[parameter]);*/
}
else switch(parameter){
case 'G':{ lastseg = (char)(atoi(inpbuf)); // Get value for
segnum = firstseg; // last segment.
stat.loop = 0;
stat.run = 1; // Start profile.
}
break;
/***********ADDED NEW CODE COMMAND R Run **************/
case 'R':
lastseg = firstseg; // Get value for
segnum = firstseg; // last segment.
phase1dist.i[0] = SegmentOrd[DIST];
vlim = SegmentOrd[VEL];
accell.i[0] = SegmentOrd[ACCEL];
dtime = SegmentOrd[TIME];
travel_to = SegmentOrd[TRAVEL];
SegmentOrd[DIST] = travel_to - (mposition >> 8);
stat.loop = 0;
stat.run = 1; // Start profile.
break;
/***********END NEW CODE*********/
case 'L':{ lastseg = (char)(atoi(inpbuf)); // Get value for
segnum = firstseg; // last segment.
stat.loop = 1; // Enable looping
stat.run = 1; // Start profile
}
break;
default: break;
}// else switch(parameter){
//}// if(!stat.run){
}// else if(comcount == 2){
else;
}
//-------------------------------------------------------------------
// ExtraeCommand()
// Processes incoming USART data.
//-------------------------------------------------------------------
void ExtraeCommand(char Caracter){
//if(bit_test(PIR1,RC1IF)){ // Check for USART interrupt
switch(Caracter){
case ',': DoCommand(); // process the string
memset(inpbuf,0,8); // clear the input buffer
i = 0; // clear the buffer index
comcount++; // increment comma count
break;
case 0x0d: DoCommand(); // process the string
memset(inpbuf,0,8); // clear the input buffer
i = 0; // clear the buffer index
comcount = 0; // clear comma count
segnum = 0; // clear segment number
parameter = 0; // clear paramater
break;
default: inpbuf[i] = Caracter, //udata; // get received char
i++; // increment buffer index
if(i > 7){ // If more than 8 chars received before getting
memset(inpbuf,0,8); // buffer
i = 0; // the buffer index
}
else; // {TXREG1 = udata; } //echo character
break;
}//end switch(udata)
}
//////////////////////////////////////
/////////////////////////////////////
void CargaDistancia(signed int16 Distt){
//segment2[segnum - 12][parameter] = atol(inpbuf);
SegmentOrd[DIST] = Distt; //Distancia
SegmentOrd[VEL] = 4096; //Velocidad
SegmentOrd[ACCEL] = 32000; //Aceleración
SegmentOrd[TIME] = 0; //Time
}
//////////////////////////////////////
/////////////////////////////////////
void AnchoBanda(void){
PORTD = 0; // Clear the LED bargraph display.
PORTE &= 0x04; // "
if(ADRES > 225){
PORTE |= 0x03; // Turn on 10 LEDS
PORTD = 0xff;
}
if(ADRES > 200){
PORTE |= 0x01; // Turn on 9 LEDS
PORTD = 0xff;
}
else if(ADRES > 175) PORTD = 0xff; // Turn on 8 LEDS
else if(ADRES > 150) PORTD = 0x7f; // 7 LEDS
else if(ADRES > 125) PORTD = 0x3f; // 6 LEDS
else if(ADRES > 100) PORTD = 0x1f; // 5 LEDS
else if(ADRES > 75) PORTD = 0x0f; // 4 LEDS
else if(ADRES > 50) PORTD = 0x07; // 3 LEDS
else if(ADRES > 25) PORTD = 0x03; // 2 LEDS
else if(ADRES > 0) PORTD = 0x01; // 1 LED
else;
}
//
// File: ZoomSYS.C
//
//---------------------------------------------------------------------
// servo_isr()
// Performs the servo calculations
//---------------------------------------------------------------------
///---------------------------------------------------------------------
// Interrupt Timer 2
// Esta interrupción sirve para que se ejecuten los controles
// cada xx ms.
// El temporizador se ajusta con el Prescaler y
// Postescaler del registro T2CON
// El PWM se ajusta solo con el Prescaler
//---------------------------------------------------------------------
#INT_TIMER2
void Timer2(){
output_high(SPULSE); // = 1; // Toggle output pin for ISR code timing
UpdTraj(); // Get new commanded position
UpdPos(); // Get new measured position
CalcError(); // Calculate new position errorr
CalcPID();
bit_clear(PIR1,TMR2IF); //PIR1bits.TMR2IF = 0; // Clear Timer2 Interrupt Flag.
output_low(SPULSE);// = 0; // Toggle output pin for ISR code timing
}
/////////////////////////////////////////////////////////////
// INT TIMER3
// La INT3 se emplea para llevar a tope el zoom y que no patine
// Con el servo MG946R en aceleración la distancia entre pulsos
// es de 12 ms máx y en frenado 22ms.
/////////////////////////////////////////////////////////////
#INT_TIMER3
void Timer3() {
set_timer3(400); //400 = 40ms. con 40MHz
if((get_timer0() == Timer0OLD) && (get_timer1() == Timer1OLD)){
TIMER3OFF();
}
else{
Timer0OLD=get_timer0();Timer1OLD = get_timer1();
}
}
/////////////////////////////////////////////////////////////
// INT RDA, RS232
// Recibe una orden através del la UART
/////////////////////////////////////////////////////////////
#ifdef PIC18LF6722
#INT_RDA2
#else
#INT_RDA
#endif
void UARTRx() {
unsigned char ByteRx,buffer;
unsigned int8 timeout;
ByteRx=fgetc(UARTR1);
#ifdef PIC18LF6722
bit_clear(PIR1,RC1IF); //Borra bandera de INT
#else
bit_clear(PIR1,RCIF);
#endif
TramaRecUART[buffer]=ByteRx;
buffer++; // Incrementa string pointer, el primero es ">"
comcount = 0;
do{
timeout =0; //Espera a recibir, y si no, solo espera 1ms después de recibir el último
while (!kbhit(UARTR1) && (++timeout<100))delay_us(10);
if(kbhit(UARTR1)) {
ByteRx = fgetc(UARTR1); } // Get un caracter de la USART
else{OrdenRec = False;break;}
TramaRecUART[buffer] = ByteRx;
buffer++; // Incrementa string pointer
//if(ByteRx == ',') comcount++; //N parametros
if(buffer >80){buffer = 0; break;}
}while(ByteRx > 9);//9 //Incluye CR y LF
OrdenRec = True;
if((ByteRx == 13)) { //|| (ByteRx == 10)){ //Incluye CR y LF
TramaRecUART[buffer] = 0;
OrdenRec = True;
}
}
//
// File: ZoomAN696.C
//
/////////////////////////////////////////////////////////////////////////////
// CONTROL DE MOTORRES PARA PLATAFORMA ///
// GIROESTABILIZADA ///
// PIC "C" Compiler PCM ///
// Esta versión es la que procede de la original del PIC18C452 ///
// Se ha añadido la comprobación, después de leer la EEPROM, ///
// si Kp y Kd son 0xFFFF. Esto indica que la EEPROM estaba ///
// borrada. Un valor 0xFFFF hace que no funcione nada ///
// Se inicializa Kp con 6000 y Kd con -1000 (0 a 6000 y -32000 a 32000)///
/// para el servo MG946R ///
///////////////////////////////////////////////////////////////////////////////
//#define PIC18LF6722
#define PLLMHz
#define Debuger1
#ifdef PIC18LF6722
#include <18F6722.h>
#include <p18f6722m.inc>
#else
#include <18F8722.h>
#include <p18f8722m.inc>
#endif
#device *=16
#include "STRING.H"
#include "STDLIB.H"
#include <ZoomAN696.h>
#include <ZoomSubPID.c>
#include <ZoomSUB.c>
#include <ZoomSYS.c>
#ZERO_RAM
//#priority ext, timer1,timer3
//---------------------------------------------------------------------
// main()
//---------------------------------------------------------------------
void main(){
unsigned int8 CaracterRec,NCaracterRec;
fprintf(UARTR1,"Configurando PIC18xx...\r\n");
Setup();
fprintf(UARTR1,"PWM On\r\n");
EnableDriverPW();ENBMotor=True; //RE2 = 1;
SegmentOrd[DIST] = 450;
//CargaDistancia(450);
SegmentOrd[VEL] = 20; //10 4096Velocidad
//SegmentOrd[ACCEL] = 32000; //Aceleración
SegmentOrd[ACCEL] = 50; //6000 Acelera
SegmentOrd[TIME] = 0; //Time
firstseg =23;lastseg=23;
stat.loop = 0;
stat.run = 1;
delay_ms(100); //Espera que arranque
TIMER3ON();
do{
delay_us(100);
}while(bit_test(T3CON,TMR3ON));
//stat.run = 0;
// set_timer0(450);
SegmentOrd[DIST] = 0;
mposition = position =fposition;
stat.motion = flatcount = velact = dtime = 0; stat.motion = 0;
fprintf(UARTR1,"mpos: %Ld fpos: %Ld posi: %Ld\r\n",mposition,fposition,position);
fprintf(UARTR1,"T0: %Ld T1: %Ld\r\n",get_timer0(),get_timer1());
delay_ms(100); //Espera que arranque
set_timer1(0);
set_timer0(0);
UpCount = DnCount = 0;
SegmentOrd[DIST] = -450;
//CargaDistancia(-450);
firstseg =23;lastseg=23;
stat.loop = 0;
stat.run = 1;
delay_ms(100); //Espera que arranque
TIMER3ON();
do{
delay_us(100);
}while(bit_test(T3CON,TMR3ON));
PosicionReal = get_timer0() - get_timer1();
// set_timer1(450);
SegmentOrd[DIST] = 0;
mposition = position =fposition;
stat.motion = flatcount = velact = dtime = 0; stat.motion = 0;
delay_ms(50); //250
set_timer1(0); set_timer0(0); PosicionReal = 0;
DnCount = UpCount = flatcount = velact = dtime = 0; stat.motion = 0;
OrdenPosOLD = OrdenPosNEW = SegmentOrd[DIST] = 0;
////////////////////////////////////////////////////
///////////////////////////////////////////////////
while(1){
restart_wdt();
bit_clear(PIR1,ADIF); //Borra INT
bit_set(ADCON0,GO); // Start an A/D conversion
while(!bit_test(PIR1,ADIF)); //while(ADGO); // Wait for the conversion to complete
ValorADC = (float)ADRESHL;
ValorADC = (ValorADC*100)/1024;
OrdenPosADC = (unsigned int16)ValorADC;
if(ValorADC >60) PORTD = 0b10000000;
else if(ValorADC >55) PORTD = 0b01000000;
else if(ValorADC >50) PORTD = 0b00100000;
else if(ValorADC >47) PORTD = 0b00010000;
else if(ValorADC >45) PORTD = 0b00001000;
else if(ValorADC >40) PORTD = 0b00000100;
else if(ValorADC >35) PORTD = 0b00000010;
else if(ValorADC >30) PORTD = 0b00000001;
else;
// fprintf(UARTR1,"New: %ld, %ld\r\n",OrdenPosNEW,PosicionReal);
if(OrdenRec){
NCaracterRec = strlen(TramaRecUART);
for(CaracterRec=0;CaracterRec<NCaracterRec;CaracterRec++){
ExtraeCommand(TramaRecUART[CaracterRec]);
}
OrdenRec = False;
}
}//end while(1)
}
|
  |
|
|
RUFINOGG
Joined: 15 Jan 2010
Posts: 8
|
|
| Posted: Wed Dec 11, 2013 3:44 am |
|
|
Thanks for the information. I can not prove until the end of January, all I can do is study it, when you try it will tell you what happens.
Thanks |
|
|
RUFINOGG
Joined: 15 Jan 2010
Posts: 8
|
|
| Posted: Wed Dec 11, 2013 4:48 am |
|
|
Hi Jerry I,
I am studying the changes you've made and see that SetupMove(){ has added:
/ *********** ADDED NEW CODE ************** /
travel_to = SegmentOrd [TRAVEL]
/ *********** END NEW CODE ********* /
but is useless because "travel_to" is not used. Only use in:
case 'R':
...
travel_to = SegmentOrd [TRAVEL];
SegmentOrd [DIST] = travel_to - (mposition >> 8);
....
must be missing something |
|
|
Jerry I
Joined: 14 Sep 2003
Posts: 96
Location: Toronto, Ontario, Canada
|
|
| Posted: Thu Dec 12, 2013 8:05 am |
|
|
Hi
Its been 2 years I last looked at code.
But from what I remember I would use the T command to enter the travel_to value. Then I would use the R (run) command to execute. R command has the same syntax as the G command.
I would only use the R command for the new code added.
Let me know if it works. |
|
|
Jerry I
Joined: 14 Sep 2003
Posts: 96
Location: Toronto, Ontario, Canada
|
|
| Posted: Thu Dec 12, 2013 2:40 pm |
|
|
Hi
Found other code where segment data is defined, and also assign a value
| Code: |
//******** NEW ADDED Code Changed **************/
// signed int16 SegmentOrd[4]; // array in bank3 for 12 motion segments
signed int16 SegmentOrd[5]; // array in bank3 for 12 motion segments
//******** NEW ADDED **************/
void main(){
unsigned int8 CaracterRec,NCaracterRec;
fprintf(UARTR1,"Configurando PIC18xx...\r\n");
Setup();
fprintf(UARTR1,"PWM On\r\n");
EnableDriverPW();ENBMotor=True; //RE2 = 1;
SegmentOrd[DIST] = 450;
//CargaDistancia(450);
SegmentOrd[VEL] = 20; //10 4096Velocidad
//SegmentOrd[ACCEL] = 32000; //Aceleración
SegmentOrd[ACCEL] = 50; //6000 Acelera
SegmentOrd[TIME] = 0; //Time
//******** NEW ADDED **************/
SegmentOrd[TRAVEL] = ??????; //Encoder value to Travel to
//******** NEW ADDED **************/
void CargaDistancia(signed int16 Distt){
//segment2[segnum - 12][parameter] = atol(inpbuf);
SegmentOrd[DIST] = Distt; //Distancia
SegmentOrd[VEL] = 4096; //Velocidad
SegmentOrd[ACCEL] = 32000; //Aceleración
SegmentOrd[TIME] = 0; //Time
//******** NEW ADDED **************/
SegmentOrd[TRAVEL] = ??????; //Encoder value to Travel to
//******** NEW ADDED **************/
}
|
Let me know what you find. |
|
|
|
|
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
