// leonardo torres

//yafar hernandez
// PRUEBA-PWM

// LCD module connections

sbit LCD_RS at RD0_bit;
sbit LCD_EN at RD1_bit;
sbit LCD_D4 at RD2_bit;
sbit LCD_D5 at RD3_bit;
sbit LCD_D6 at RD4_bit;
sbit LCD_D7 at RD5_bit;

sbit LCD_RS_Direction at TRISD0_bit;

sbit LCD_EN_Direction at TRISD1_bit;
sbit LCD_D4_Direction at TRISD2_bit;
sbit LCD_D5_Direction at TRISD3_bit;
sbit LCD_D6_Direction at TRISD4_bit;
sbit LCD_D7_Direction at TRISD5_bit;
// End LCD module connections

sbit M11 at RB0_bit;

sbit M12 at RB1_bit;

sbit M21 at RB2_bit;

sbit M22 at RB3_bit;

//funcion velocidad de motor 2

sbit sube2 at RB6_bit;
sbit baja2 at RB7_bit;
//grio motor 2
sbit giro1 at RB4_bit;
sbit giro2 at RB5_bit;

char texto[20];

// variables
unsigned int codigo;
unsigned int velo1;
signed int velo2=0;
//motor1
void floattostr_(float numero_, unsigned char *cadena_,char decimales_)
{
//variables temporales
int largo_entera,largo_n,cont_for,tempo_int;
double tempo_float;
//largo de la trama a armar en decimales
largo_n = decimales_+1;
largo_entera = 0;
// si es negativa coloca el -
if ( numero_ < 0)
{
*cadena_++ = '-';
numero_ = -numero_;
}
//si es menor que no multiplica por 10
if (numero_ > 0.0) while (numero_ < 1.0)
{
numero_ =numero_* 10.0;
 largo_entera--;
}

//realiza division varias veces hasta que sea menor que 10

while (numero_ >= 10.0)
{
numero_ = numero_/10.0;
largo_entera++; //sube el largo de la trama por ser deciaml
}
largo_n = largo_n+largo_entera; //el largo es la decimal mas la parte entera

//round. numero_ is between 1 and 10 and largo_n will be printed to

// right of decimal point so rounding is ...
for (tempo_float = cont_for = 1; cont_for < largo_n; cont_for++)
tempo_float = tempo_float/10.0;
numero_ += tempo_float/2.0;
if (numero_ >= 10.0) {numero_ = 1.0; largo_entera++;}
//si tiene decimales
if (largo_entera<0)
{
*cadena_++ = '0'; *cadena_++ = '.';
if (largo_n < 0) largo_entera = largo_entera-largo_n;
for (cont_for = -1; cont_for > largo_entera; cont_for--) *cadena_++ = '0';
}
for (cont_for=0; cont_for < largo_n; cont_for++)
{
tempo_int = numero_;
*cadena_++ = tempo_int + 48; //convierte a ascci
if (cont_for == largo_entera ) *cadena_++ = '.';
numero_ -= (tempo_float=tempo_int);
numero_ = numero_*10.0;
}

}
//motor 1

void derecha1()
{
M11=0; //izquierda
M12=1; //derecha
}
void izquierda1()
{
M11=1; //izquierda
M12=0; //derecha
}

//motor2
void derecha2()
{
M21=0;
M22=1;
}
void izquierda2()
{
 M21=1;
M22=0;
}

void main()
{
ADCON1=0B1110; //ANALOGO
TRISA=0B11111111; // A ENTRADA
TRISB=0; // todo salida

// ENTRADA DE MOTOR

TRISB4_bit=1;
TRISB5_bit=1;
TRISB6_bit=1;
TRISB7_bit=1;

// SALIDAS PWM
RC1_bit=0;
RC2_bit=0;

// FRECUENCIAS DEL PWM

PWM1_Init(500);
PWM2_Init(500);
PWM1_Set_Duty(40);
PWM2_Set_Duty(200);

//ARRANCA PWM

PWM1_Start(); // ARRANCA
PWM2_Start();

ADC_Init();

Lcd_Init(); // Initialize LCD

Lcd_Cmd(_LCD_CLEAR); // Clear display

Lcd_Cmd(_LCD_CURSOR_OFF); // Cursor off

// ESCRITURA DEL DISPLAY

Lcd_Out(1,1,"ve1="); //velocidad1
Lcd_Out(3,1,"ve2="); //velocidad2
Lcd_Out(1,12,"G1="); //giro1
Lcd_Out(3,12,"G2="); //giro2

NOT_RBPU_bit=0; // habilito pull up porB entrada

while(1)

{
// ecuacion de velocidad
codigo=ADC_Read(0);
velo1= (codigo*255.0/1023.0);
PWM1_Set_Duty (velo1);
 // giros del motor
derecha1();
izquierda2();

//control de velocidad digital motor 2

if(sube2==0)
{
velo2=velo2+10;
if(velo2>=255) velo2=255;
PWM2_Set_Duty (velo2);
Delay_ms(200);

}
if(baja2==0)
{
velo2=velo2-10;
if(velo2<=0) velo2=0;
PWM2_Set_Duty (velo2);
Delay_ms(250);

}
if(giro1==0)
{
izquierda1();
Lcd_Chr(1,15,'I');
}
else

{
derecha1();
Lcd_Chr(1,15,'D');
}

if(giro2==0)
{
izquierda2();
Lcd_Chr(3,15,'I');
}
else

{
derecha2();
Lcd_Chr(3,15,'D');
}

floattostr_(velo1*100.0/255.0,texto,1);
Lcd_Out(1,5,texto);
floattostr_(velo2*100.0/255.0,texto,2);
Lcd_Out(3,5,texto);
}
}