CPIDGuard::CPIDGuard(const string& filename, const string& dir)
: m_OldPID(0), m_NewPID(0)
{
string real_dir;
CDirEntry::SplitPath(filename, &real_dir, 0, 0);
if (real_dir.empty()) {
if (dir.empty()) {
real_dir = CDir::GetTmpDir();
} else {
real_dir = dir;
}
m_Path = CDirEntry::MakePath(real_dir, filename);
} else {
m_Path = filename;
}
UpdatePID();
}
void servoisr()
{
static int update;
if (update>15){
update = 0;
if (STEP_ENABLE){
SYNC = true;
UpdatePosition();
UpdateTrajectory();
CalculateExtCount();
UpdatePID();
UpdatePWM();
SYNC = false;
}
rx_timer++;
} else update++;
}
void pidSetControl()
{ int j;
// 0 = right side
for(j=0; j < NUM_PIDS; j++)
{ //pidobjs[0] : right side
// p_input has scaled velocity interpolation to make smoother
// p_state is [16].[16]
pidObjs[j].p_error = pidObjs[j].p_input + pidObjs[j].interpolate - pidObjs[j].p_state;
pidObjs[j].v_error = pidObjs[j].v_input - pidObjs[j].v_state; // v_input should be revs/sec
//Update values
UpdatePID(&(pidObjs[j]));
} // end of for(j)
if(pidObjs[0].onoff && pidObjs[1].onoff) // both motors on to run
{
tiHSetDC(1, pidObjs[0].output);
tiHSetDC(2, pidObjs[1].output);
}
else // turn off motors if PID loop is off
{ tiHSetDC(1,0); tiHSetDC(2,0); }
}
void main()
{
InitHardware();
Encoder(true);
ResetServo();
delay_ms(200);
bEnable = !STEP_ENABLE;
// run these asynch tasks
for (;;){
if (bServo){
SYNC = true;
UpdatePosition();
UpdateTrajectory();
CalculateExtCount();
UpdatePID();
UpdatePWM();
HandleErrorTask();
bServo = false;
} else {
//HandleRXData();
if (SYNC){
if (bDirty && !bEnable){
bDirty = false;
SaveEEPROM();
}
SYNC = false;
}
}
}
}
int main(void)
{
//========= Peripheral Enable and Setup ===========//
setup_LCD(); //LCD Screen setup
setup_tmp36(); //TMP36 ADC thermometer setup
setup_GPS(); //GPS Pin Setup
setup_microSD(); //MicroSD Pin Setup
setupTMP102();
setup_PID(&pid);
setupBMS();
setup_pwm();
//Setup the buttons
setup_buttonInterrupts();
sleepEnablePeripherals();
setup_timerInterrupt();
//========= Peripheral enable and run ============//
enable_LCD(); //Start LCD Commmunication
enable_GPS(); //Start GPS Communication
clearDisplay(); //Refresh Display
while(1)
{
//Listen for the GPS Data
listen_GPS();
//Open the datalog file for writing
open_datalog();
//Obtain vehicle speed
char *velocity;
char velocityArray[] = "000";
//velocity = velocityArray;
velocity = getVelocity();
//Convert string to int
int speedInteger = atoi(velocity);
//Convert knots to mph
speedInteger = 1.15 * speedInteger;
//Return to string
sprintf(velocity, "%i", speedInteger);
// velocity = "50";
selectLineOne();
//Show velocity
putPhrase("V:");
putPhrase(velocity);
//Analog Temperature Sensor
int anag_tempValue = get_analog_temp();
sprintf(anag_temp, "%i", anag_tempValue);
//Digital Temperature Sensor
int digi_tempValue = getTemperature();
sprintf(digi_temp, "%i", digi_tempValue);
//Get BMS Level
char load[] = "89";
char *load_value;
load_value = load;
//-------BMS Communication------
sendStartSequence();
getDataString();
load_value = getLoad();
//------------------------------
//If Cruise Control is on
if(enableSys)
{
//If initialized
if(obtain_speed)
{ //Debouncing
//SysCtlDelay(533333);
obtain_speed = 0;
clearDisplay();
//Get set velocity
set_velocity = 10;
}
//If driver increases set speed
if(incr_speed)
{
//SysCtlDelay(533333);
incr_speed = 0;
set_velocity += 1;
}
//if driver decresaes set speed
else if(decr_speed)
{
//SysCtlDelay(533333);
decr_speed = 0;
set_velocity -= 1;
}
//==============PID Portion of the Main Loop ==============//
float process_value = atof(velocity); //Convert String to Int
float error = set_velocity - process_value; //Calculate error
float u_t = UpdatePID(&pid, error, 1.0); //Feed error to the PID
uint32_t duty_cycle = u_t*scaleFactor;
if(duty_cycle < 950){
duty_cycle = 50;
}
else if(duty_cycle > 0){
duty_cycle = 950;
}
pwm_out(1000, duty_cycle); //Scale and output the PID output
//====================================================//
//Show other essentials to the LCD
putPhrase("mph/");
char set_point[5];
sprintf(set_point, "%imph", (int) set_velocity);
putPhrase(set_point);
putPhrase(" ON"); //Cruise control on
}
else
{
//Spacer
putPhrase("mph ");
putPhrase("Standby ");
}
//Select bottom line
selectLineTwo();
//Display analog and digital temperatures
putPhrase("T:");
putPhrase(anag_temp);
putPhrase("/");
putPhrase(digi_temp);
putPhrase("C ");
//Show Load level
putPhrase("B:");
putPhrase(load_value);
putPhrase("%");
write_datalog(velocity, "ddmm.mmmmmmX", "ddmm.mmmmmX", getTime(), anag_temp, digi_temp, load_value);
close();
//Put system in low power mode
SysCtlSleep();
}
}
int main() {
char select[10];
char comando[100];
struct pollfd *ufds;
pthread_t muestreo;
int Kp,Kd,Ki;
float T=0.0001; // tiempo de muestreo
int Tp, Td, Ti;
int ret;
int leido=0;
int sensores2,aux;
double position;
SPid pid;
int i;
int error_anterior=0;
int pos_servo=0x8C, integral=0;
int flag=-1;
int est90=0;
int estadobifur=0;
int dist, dist_anterior;
int bifur;
int kaka=0;
int kambio=0;
int sensores_anterior=0;
int flag_hasta_blanco=0;
//int flag_ignorar=0;
signal(SIGINT,handler);
fd=serial_open("/dev/ttyS0");
pid.iMax=125; pid.iMin=1;
pid.pGain = 1.2;
pid.dGain = 0.0;
pid.iGain = 0.0;
position = 0x8C;
pid.dState=0;
pthread_create(&muestreo,NULL,muestrear,NULL);
comando[0]=(unsigned char)255;
comando[1]='\r';
comando[2]=130;
comando[3]='\r';
comando[4]='\0';
enviar(fd,comando,5);
while(flag_salir!=1) {
sensores_anterior=sensores;
sensores=sensoresPIC;
/*if (!(BIT_TEST(sensores,3) && BIT_TEST(sensores,2) && BIT_TEST(sensores,1)) && (kambio)) {
borde = kambio;
kambio = 0;
flag_centro=0;
}*/
/* Modificaciones */
/* if( borde == DCHO && BIT_TEST(sensores,0) ) {
error=-10.0; // girar mogollón a la derecha
position+=UpdatePID(&pid, error, error);
comando[0]=(unsigned int)position;
comando[1]='\r';
comando[2]='\0';
enviar(fd,comando,strlen(comando)); // Enviamos la corrección calculada al PIC
continue;
}
else {
if( borde == IZQDO && BIT_TEST(sensores,7) ) {
error=10.0; // girar mogollón a la derecha
position+=UpdatePID(&pid, error, error);
comando[0]=(unsigned int)position;
comando[1]='\r';
comando[2]='\0';
enviar(fd,comando,strlen(comando)); // Enviamos la corrección calculada al PIC
continue;
}
} */
/* End modificaciones */
bifur = detect(sensores,0,7);
switch(estadobifur) {
case 0:
if(bifur == 1 )
estadobifur=1;
break;
case 1:
if(bifur == 0 ) {
/* if(flag_ignorar == 0 ) {
printf("marca\n");*/
//if( !detect(sensores_anterior,0,7) ) {
borde=flag_bifurcando;
//kambio = flag_bifurcando;
flag_centro=0;
estadobifur=0;
//}
//flag_ignorar=1;
}
break;
}
/* if (!(BIT_TEST(sensores,3) && BIT_TEST(sensores,2) && BIT_TEST(sensores,1)) && (kambio)) {
borde = kambio;
kambio = 0;
flag_centro=0;
} */
/* Modificaciones */
//if ( borde == DCHO && BIT_TEST(sensores,3) && BIT_TEST(sensores,1) && BIT_TEST(sensores,2) && !detect(sensores_anterior,0,7) && (!(BIT_TEST(sensores_anterior,3) && BIT_TEST(sensores_anterior,1) && BIT_TEST(sensores_anterior,2)) ) ) {
if ( borde == DCHO && BIT_TEST(sensores,3) && BIT_TEST(sensores,1) && BIT_TEST(sensores,2) && !detect(sensores_anterior,0,7) && (!(BIT_TEST(sensores_anterior,3) && BIT_TEST(sensores_anterior,1) && BIT_TEST(sensores_anterior,2)) ) ) {
printf("movida\n");
error=-40.0;
position+=UpdatePID(&pid, error, error);
if( position >= 250.0 )
position =250.0;
else
if( position <= 5.0 )
position =5.0;
//position-=80.0;
comando[0]=(unsigned int)position;
comando[1]='\r';
comando[2]='\0';
enviar(fd,comando,strlen(comando)); // Enviamos la corrección calculada al PIC
//usleep(200000);
continue;
}
/* end mod */
if ( borde == IZQDO && BIT_TEST(sensores,6) && BIT_TEST(sensores,5) && BIT_TEST(sensores,4) && !detect(sensores_anterior,0,7) && (!(BIT_TEST(sensores_anterior,6) && BIT_TEST(sensores_anterior,5) && BIT_TEST(sensores_anterior,4)) ) ) {
printf("movida\n");
error=40.0;
position+=UpdatePID(&pid, error, error);
if( position >= 250.0 )
position =250.0;
else
if( position <= 5.0 )
position = 5.0;
//position-=80.0;
comando[0]=(unsigned int)position;
comando[1]='\r';
comando[2]='\0';
enviar(fd,comando,strlen(comando)); // Enviamos la corrección calculada al PIC
//usleep(200000);
continue;
}
/*if( flag_hasta_blanco==1 ) {
if( !BIT_TEST(sensores,3) && !BIT_TEST(sensores,4) && !BIT_TEST(sensores,2) && !BIT_TEST(sensores,5) )
flag_hasta_blanco = 0;
else
continue;
}*/
switch(borde) {
case DCHO:
if( (BIT_TEST(sensores,3) == 1) && (BIT_TEST(sensores,2) == 0 ) ) { /* 10 */
flag_centro=0;
error=0.0;
if( bifur ) {
if( detect(sensores,3,7) )
flag_bifurcando =IZQDO;
else
flag_bifurcando = DCHO;
}
continue;
}
if( (BIT_TEST(sensores,3) == 0) && (BIT_TEST(sensores,2) == 1 ) ) { /* 01 */
error=(float)-1.0*1;
flag_centro=1;
}
if( (BIT_TEST(sensores,3) == 0) && (BIT_TEST(sensores,2) == 0) ) { /* 00 */
if(flag_centro) {
error=(float)-1.0*1;
if(bifur) {
if( detect(sensores,0,2) )
flag_bifurcando=DCHO;
else
flag_bifurcando=IZQDO;
}
}
else {
error=(float)1;
if(bifur) {
if( detect(sensores,3,7) )
flag_bifurcando=IZQDO;
else
flag_bifurcando=DCHO;
}
}
/* ultima hora */
if(!bifur) {
if( sensores > 0x0F) {
aux=0;
sensores2 = sensores >> 4;
for(i=0;i<4;i++) {
if ( BIT_TEST(sensores2,i) )
aux=BIT_SET(aux,3-i);
}
error = (float)tabla[aux];
}
else {
if( sensores < 0x0F )
error = (float)-1.0*tabla[sensores];
}
}
/* fin ultima hora */
}
if( (BIT_TEST(sensores,3) == 1) && (BIT_TEST(sensores,2) == 1) ) { /* 11 */
error=(float)-1.0*1;
flag_centro =1;
if( bifur ) {
if( detect(sensores,3,7) )
flag_bifurcando = IZQDO;
else
flag_bifurcando = DCHO;
}
}
break;
case IZQDO:
// error negativo = girar dcha
if( (BIT_TEST(sensores,5) == 0) && (BIT_TEST(sensores,4) == 1 ) ) { /* 01 */
flag_centro=0;
if( bifur ) {
if( detect(sensores,0,4) )
flag_bifurcando = DCHO;
else
flag_bifurcando = IZQDO;
}
continue;
}
if( (BIT_TEST(sensores,5) == 1) && (BIT_TEST(sensores,4) == 0 ) ) { /* 10 */
error=(float)1;
flag_centro=1;
}
if( (BIT_TEST(sensores,5) == 0) && (BIT_TEST(sensores,4) == 0) ) { /* 00 */
if(flag_centro) {
error=(float)1;
if(bifur) {
if( detect(sensores,5,7) )
flag_bifurcando=IZQDO;
else
flag_bifurcando=DCHO;
}
}
else {
error=(float)-1.0*1;
if(bifur) {
if( detect(sensores,0,3) )
flag_bifurcando=DCHO;
else
flag_bifurcando=IZQDO;
}
}
/* ultima hora */
if(!bifur) {
if( sensores > 0x0F) {
aux=0;
sensores2 = sensores >> 4;
for(i=0;i<4;i++) {
if ( BIT_TEST(sensores2,i) )
aux=BIT_SET(aux,3-i);
}
error = (float)tabla[aux];
}
else {
if( sensores < 0x0F )
error = (float)-1.0*tabla[sensores];
}
}
