#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <plib/timers.h>
#include <plib/usart.h>
#include <plib/pwm.h>
#include <stdbool.h>
#include "TractorBot2_Master.h"
#include "TractorBot2.h"

// PIC18F46K22 configurations

#pragma config FOSC=INTIO67,PLLCFG=ON,PRICLKEN=OFF,FCMEN=OFF,IESO=OFF,PWRTEN=ON
#pragma config BOREN=OFF,BORV=190,WDTEN=OFF,WDTPS=512,PBADEN=OFF,MCLRE=INTMCLR
#pragma config STVREN=OFF,LVP=OFF,DEBUG=OFF,CP0=OFF,CP1=OFF,CP2=OFF,CP3=OFF
#pragma config CPB=OFF,CPD=OFF,WRT0=OFF,WRT1=OFF,WRT2=OFF,WRT3=OFF,WRTC=OFF
#pragma config WRTB=OFF,WRTD=OFF,EBTR0=OFF,EBTR1=OFF,EBTR2=OFF,EBTR3=OFF,EBTRB=OFF

// Pin definitions

// RA0-RA7 = LCD
#define IR                  PORTBbits.RB0   // IR-receiver for remote control systems SFH 5110-38
#define US_FWD_OUT          LATBbits.LATB1
#define US_FWD_IN           PORTBbits.RB2

#define GE                  LATCbits.LATC0
#define RX0                 PORTCbits.RC2   //RX=reflective sensor
#define ENC2                PORTCbits.RC1
#define RX2                 PORTCbits.RC3
#define US_TOP_OUT          LATCbits.LATC4
#define US_TOP_IN           PORTCbits.RC5

#define RX3                 PORTDbits.RD0
#define RX4                 PORTDbits.RD1
#define RX1                 PORTDbits.RD3
//RD6 UART TX
//RD7 UART RX

#define GCS1               LATEbits.LATE0
#define GCS2               LATEbits.LATE1
#define GDI                LATEbits.LATE2
#define SW                 PORTEbits.RE3

#define Motor_control_forward   if(motorGo!=Uforward)   {sprintf(UARTMessage, "%04i%04i", Uforward,   Ublemc); motorGo=Uforward;    while(Busy2USART()); puts2USART(UARTMessage);}
#define Motor_control_reverse   if(motorGo!=Ureverse)   {sprintf(UARTMessage, "%04i%04i", Ureverse,   Ublemc); motorGo=Ureverse;    while(Busy2USART()); puts2USART(UARTMessage);}
#define Motor_control_TurnLeft  if(motorGo!=UTurn_left) {sprintf(UARTMessage, "%04i%04i", UTurn_left, Ublemc); motorGo=UTurn_left;  while(Busy2USART()); puts2USART(UARTMessage);}
#define Motor_control_TurnRight if(motorGo!=UTurn_right){sprintf(UARTMessage, "%04i%04i", UTurn_right,Ublemc); motorGo=UTurn_right; while(Busy2USART()); puts2USART(UARTMessage);}
#define Motor_control_Stop      if(motorGo!=UStop_drive){sprintf(UARTMessage, "%04i%04i", UStop_drive,Ublemc); motorGo=UStop_drive; while(Busy2USART()); puts2USART(UARTMessage);}

#define s       1000000
#define ms      1000
#define us      1

#define not_black  0
#define black      1

#define _XTAL_FREQ  64000000
#define USE_AND_MASKS

#define ZmotorGo 1
#define Zx       2
#define Zy       3
#define Zrx0     4
#define Zenc2    5
#define Zrx1     6
#define Zrx2     7
#define Zrx3     8
#define Zrx4     9
//#define Zlsw1    10
//#define Zlsw2    11
//#define Zrx1     12
#define Zus1     13
#define Zus2     14
#define ZRX0_bool 10
#define Zenc2_bool 11

#define remote 1
//variable declarations
bool edge=false, half=false;
int OSCFrequency,IRpulsePeriod[17];

unsigned int  TMR0_total, EncoderShort=0, EncoderTotal=0, Z[15], part=0;
unsigned long TMR0_interrupt_number,UARTcycle=0, test, Battery_voltage, Current;

unsigned char UARTConfig = 0, baud = 0, UARTMessage[20],UART_RX_Buffer[20], IRnumber=0, motorGo;
float wait_line=0;


//procedure declarations

void PICinit();
void LCDinit();
void LCDsend(unsigned int command);
void LCDwrite(unsigned int command);
void LCDposition(unsigned int position);

int  PutCharMys(int row, int column, unsigned char c);                          
int  PrintText(unsigned char row, unsigned char column, unsigned char* text);
int  PrintNumber(unsigned char row, unsigned char column, long number);

unsigned long Get_US_FWD();
unsigned long Get_US_TOP();

void Get_IR_times();
int Decode_IR_signal();

void Delay(unsigned long b);        
void Wait(float time,long unit);    
void Stopwatch_ON();                
unsigned long Stopwatch_OFF();      

void interrupt Interruption();

void RemoteControl();
void Window();
void WindowForm(unsigned char* Name);

void ReadServo(char* buffer, unsigned char len);
void sendUart(long prefix, long sufix);

void GoStraight(char where);
void GoStraightB(char nothing);
void GoStraightHalf(char where, short minus);
void TurnRightAngle(char where);
void Align(bool vice_versa);

void sendUart(long prefix, long sufix)
{
    sprintf(UARTMessage, "%04i%04i", prefix, sufix);
    while(Busy2USART());
    puts2USART(UARTMessage);
}

/******************************************************************************/
/*----------------------------------------------------------------------------*/
/******************************************************************************/
void PICinit()
{
 BSR=15;

 TRISA=0;  //all output, output=0,input=1
 ANSELA=0; //all digital, 0 = digi, 1 = analog
 PORTA=0;
 LATA=0;

 TRISB=0b101;
 ANSELB=0;
 PORTB=0;
 LATB=0;

 INTCON2=0;
 WPUB=0b00000001; //pull-up on RB0

 TRISC=0b101110;
 ANSELC=0;
 PORTC=0;
 LATC=0;

 TRISD=0b11001011;
 ANSELD=0;
 PORTD=0;
 LATD=0;

 TRISE=0b10001000;
 ANSELE=0;
 PORTE=0;
 LATE=0;
 INTCON2bits.RBPU=0;

 OSCCON=0b01110000;   // 16MHz internal oscillator
 OSCCON2=0b10000000;  // 4xPLL=64MHz
 OSCTUNEbits.PLLEN=1; //PLL on
 OSCFrequency=64;
 switch(OSCFrequency)
 {
     case 4:  T0CON=0b10001000;break;
     case 8:  T0CON=0b10000000;break;
     case 16: T0CON=0b10000001;break;
     case 32: T0CON=0b10000010;break;
     case 64: T0CON=0b10000011;break;
 }

 T1GCON=0;
 T1CON=00110001 ; // set TMR1 with 1:8 prescaler and FOSC/4 clock (0.5 us per tick)

 VREFCON0=0b10010000; //setting voltage reference, range 1.024V
 while(VREFCON0==0b10010000); //waiting for setting voltage reference

 ADCON2=0b10111110; // right adjustment, delay 20TAD, frequency conversion FOSC/64
 ADCON1=0b00001000; // internal voltage reference

 // UART2 configuration
    UARTConfig = USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH;
    baud = 34;
    PIR3bits.RC2IF=0;  // interrupt UARTflag
    Open2USART(UARTConfig,baud);

 // setting interrupts
    INTCONbits.GIE=1;  // global enable all interrupts
    INTCONbits.INT0IE=1; //  INT0 interrupts enable
    INTCONbits.INT0IF=0;  // clear INT0 interrupt UARTflag
    INTCON2bits.INTEDG0=0; // interrupts at INT0 from 1 to 0 (falling edge)
 //   INTCONbits.PEIE=1;  // enable peripheral interrupts for TMR1
}

//******************************************************************************
//OLD TIMING PROCEDURE
void Delay(unsigned long b)
{
  b=b*20;
  for(;b>0;b--);
}
//******************************************************************************
//LCD DISPLAY INITIATION
void LCDinit()
{
        int i,ii;
  	GCS1=0;
	GCS2=0;			// 1 = half OFF, 0 = half of display is ON
   	LCDposition(0b00111111); // zapni displej

	GCS1=0;
	GCS2=1;
	for(ii=184;ii<192;ii++)			// clear display
	{
		LCDposition(ii);
		for(i=64;i<128;i++)
		{
			LCDwrite(255);
                }
	}

	GCS1=1;
	GCS2=0;
	for(ii=184;ii<192;ii++)
	{
		LCDposition(ii);
		for(i=64;i<128;i++)
		{
			LCDwrite(255);
                }
	}
}

//******************************************************************************
// LCD DISPLAY CONTROL
void LCDposition(unsigned int position)
{
  GDI=0;
  LCDsend(position);
}

void LCDwrite(unsigned int command)
{
  GDI=1;
  LCDsend(command);
}

void LCDsend(unsigned int command)
{
  LATA=command;
  GE=1;
  Delay(1);
  GE=0;
  Delay(1);

}

int PutCharMys(int row, int column, unsigned char c)
{
    int i,ii,kota=0;
    GCS1=0;
    GCS2=1;			// 1 = half OFF, 0 = half of display is ON
    if(column>=253) return NULL;
    for(ii=column;ii<column+16;ii++)
    {
        if(ASCII[c][kota]==0xBB) break;
        if(ii>=64) {    ii-=64;
                        GCS1=1;
                        GCS2=0; }

        for(i=row;i<row+1;i++)
        {
                LCDposition(ii+64); // set position Y: 64=0, 128=64
                LCDposition(i+183); // set row X: 184=0, 191=7
                        LCDwrite(ASCII[c][kota]);
                kota++;
        }
    }
    return kota;
}
int PrintNumber(unsigned char row,unsigned char column,long number)
{
    short j,jj=-1;
    if(number<0) { number=-number; PutCharMys(row,column,'-'); column+=4;}
    if(number!=0) {
        for(j=((unsigned long)log10(number)+1);j>0;j--)
            jj+=PutCharMys(row,jj+column,((number/(long)pow(10,(j-1)))%10)+48); PrintText(row,column+jj," ");}
    else PutCharMys(row,column,'0');
    return column+jj;
}

int PrintText(unsigned char row, unsigned char column, unsigned char* text)
{
    int ij=0,g=strlen(text)+1;
    for(int j=0; j<g; j++)
    {
        ij+=PutCharMys(row, column+ij, PMp);
        text += 1*sizeof(char);
    }
    PutCharMys(row,column+ij,' ');
    return column+ij;
}

void demo()
{
    PrintText(1,25,"TRACTORBOT");
    PrintText(3,20,"BOBESH 2.1.0.0");
    PrintText(6,20,"AHOJ OPAVO !     :-)");
    PrintText(8,8, "Zdravi Frenstat a okoli !");

    sprintf(UARTMessage, "%i%i", Shello, Ublemc);
    while(Busy2USART());
    puts2USART(UARTMessage);
    Wait(1,s);

    sprintf(UARTMessage, "%i%i", Shello, Ublemc);
    while(Busy2USART());
    puts2USART(UARTMessage);
    Wait(1,s);

    sendUart(StepRight, Ublemc);
    Wait(350,ms);
    sendUart(StepLeft, Ublemc);
    Wait(350,ms);
    sendUart(StepLeft, Ublemc);
    Wait(350,ms);
    sendUart(StepRight, Ublemc);
    Wait(350,ms);

    sendUart(GripperOpen,Ublemc);
    Wait(1,s);
    sendUart(GripperClose,Ublemc);
    Wait(1,s);

    sendUart(UTurn_right, Ublemc);
    Wait(3.5,s);
    sendUart(UTurn_left, Ublemc);
    Wait(7,s);
    sendUart(UTurn_right, Ublemc);
    Wait(3.5,s);
    sendUart(UStop_drive, Ublemc);

    sprintf(UARTMessage, "%i%i", SOk, Ublemc);
    while(Busy2USART());
    puts2USART(UARTMessage);
}

//***********************************************************************************************************************************************************
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//***********************************************************************************************************************************************************
void main()
{
    PICinit(); 	//setting ports and basic registers
    Delay(150); // Wait 17 ms for power stabilization
    LCDinit(); // clearing display

    sendUart(Power,Ublemc);
    while(Busy2USART());
    ReadServo(UART_RX_Buffer,9);
    test=atol(UART_RX_Buffer);

    Battery_voltage=((test/10000)*15260)/1000;
    Current=((test%10000)*854)/1000;

    WindowForm("Ketchup House");
    Wait(100,ms);

    sendUart(Shello,Ublemc);

    while(SW==1) Window();
    while(SW==0);

    INTCONbits.INT0IE=0; //  INT0 interrupts disable

    wait_line=0;

    GoStraight(1);TurnRightAngle('>');GoStraight(1);GoStraight(1);TurnRightAngle('<');GoStraight(1);GoStraightHalf(1,2);TurnRightAngle('<');GoStraight(1);GoStraight(1);GoStraight(1);GoStraight(1);half=true;TurnRightAngle('<');GoStraight(1);GoStraight(1);GoStraightHalf(1,0);
    GoStraightB(0);GoStraightB(0);GoStraightB(0);GoStraightHalf(0,-2);TurnRightAngle('<');GoStraight(1);GoStraight(1);GoStraight(1);GoStraight(1);half=true;TurnRightAngle('>');GoStraight(1);GoStraight(1);GoStraight(1);GoStraightHalf(1,0);
    GoStraightHalf(0,1);GoStraightB(0);GoStraightB(0);GoStraightB(0);GoStraightB(0);GoStraightHalf(0,1);half=true;TurnRightAngle('>');GoStraightB(0);GoStraight(1);GoStraight(1);GoStraight(1);GoStraight(1);GoStraight(1);GoStraight(1);//GoStraightHalf(1);
    Motor_control_Stop
    WindowForm("HAHAHAHAHA");
    sendUart(Sdog,Ublemc);
    while(1) Window();

    sendUart(GripperOpen,Ublemc);
    Wait(2,s);
}
//***********************************************************************************************************************************************************
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//***********************************************************************************************************************************************************

void Get_IR_times()
{
        const long jjj=10000;
        long j=0,jj=0;
        while(IR==1 && jj<jjj) jj++;
        jj=0;
        for(j=1;j<16;j++)
        {
            while(IR==0 && jj<jjj) jj++;
            jj=0;
            Stopwatch_ON();
            while(IR==1 && jj<jjj) jj++;
            IRpulsePeriod[j]=Stopwatch_OFF();
            jj=0;
        }
}

int Decode_IR_signal()
{
    Get_IR_times();
    int j,TotalIRcode=0;
    char IRcode[17];

    for(j=1;j<16;j++)
    {
            if(IRpulsePeriod[j]>300 && IRpulsePeriod[j]<1000) IRcode[j]=0;
            else if(IRpulsePeriod[j]>1300 && IRpulsePeriod[j]<2000) IRcode[j]=1;
            else {IRcode[j]=9;}
    }
    for(j=1;j<16;j++) TotalIRcode=TotalIRcode+IRcode[j]*(unsigned long)(pow(2,j));

    return TotalIRcode;
}

//****************************************************************************
void Wait(float timefloat, long unit)
{
     unsigned long timelong, remainingtime;
     int overflow=0;

     timelong=(unsigned long)(timefloat*unit);
     overflow=timelong/65535;
     remainingtime=65535-(timelong-(overflow*65535));

     WRITETIMER0(remainingtime);
     INTCONbits.TMR0IF=0;

    do
    {
        while(INTCONbits.TMR0IF==0);
        INTCONbits.TMR0IF=0;
        overflow--;
    }
    while(overflow>=0);
}

void Stopwatch_ON()
{
    INTCONbits.TMR0IE=1;
    INTCONbits.TMR0IF=0;
    TMR0_interrupt_number=0;
    WRITETIMER0(0);
}

unsigned long Stopwatch_OFF()
{
    TMR0_total=READTIMER0();
    INTCONbits.TMR0IE=0;
    INTCONbits.TMR0IF=0;

    return ((TMR0_interrupt_number*65536)+ TMR0_total);
}
///*****************************************************************************
void interrupt Interruption(void)
{
    //unsigned long num,j,jj;
    if(INTCONbits.INT0IF==1 && INTCONbits.INT0IE==1)
    {
       INTCONbits.INT0IE=0;
       switch(Decode_IR_signal())
       {
           case CTR_B2: IRnumber=remote; break;

           case CTR_A1:
                sprintf(UARTMessage, "%04i%04i", Uforward, Ublemc);
                motorGo=Uforward;
                while(Busy2USART());
                puts2USART(UARTMessage);
                break;
           case CTR_A2:
                sprintf(UARTMessage, "%04i%04i", UStop_drive, Ublemc);
                motorGo=UStop_drive;
                while(Busy2USART());
                puts2USART(UARTMessage);
                break;
           case CTR_A3:
                sprintf(UARTMessage, "%04i%04i", Ureverse, Ublemc);
                motorGo=Ureverse;
                while(Busy2USART());
                puts2USART(UARTMessage);
                break;
           case CTR_B1:
                sprintf(UARTMessage, "%04i%04i", UTurn_left, Ublemc);
                motorGo=UTurn_left;
                while(Busy2USART());
                puts2USART(UARTMessage);
                break;
           case CTR_C1:
                sprintf(UARTMessage, "%04i%04i", UStop_drive, Ublemc);
                motorGo=UStop_drive;
                while(Busy2USART());
                puts2USART(UARTMessage);
                break;
           case CTR_D1:
                sprintf(UARTMessage, "%04i%04i", UTurn_right, Ublemc);
                motorGo=UTurn_right;
                while(Busy2USART());
                puts2USART(UARTMessage);
                break;

           case CTR_B3:
                sprintf(UARTMessage, "%04i%04i", GripperOpen, Ublemc);
                while(Busy2USART());
                puts2USART(UARTMessage);
                break;
           case CTR_C3:
                sprintf(UARTMessage, "%04i%04i", GripperClose, Ublemc);
                while(Busy2USART());
                puts2USART(UARTMessage);
                break;
           case CTR_C6: sendUart(GripperClose,Ublemc); break;
           case CTR_A6: sendUart(GripperOpen ,Ublemc); break;
       }
       INTCONbits.INT0IE=1;
       INTCONbits.INT0IF=0;
       return;
    }

}
//******************************************************************************
unsigned long Get_US_FWD()
{
    unsigned long time_us;
    PIR1bits.TMR1IF=0;
    US_FWD_OUT=1;
    __delay_us(12);
    US_FWD_OUT=0;

    WRITETIMER1(0);
    while(US_FWD_IN==0 && PIR1bits.TMR1IF==0);
    WRITETIMER1(0);
    while(US_FWD_IN==1 && PIR1bits.TMR1IF==0);
    time_us=READTIMER1();
    return (time_us/928); //returns value in cm
}

unsigned long Get_US_TOP()
{
    unsigned long time_us;
    PIR1bits.TMR1IF=0;
    US_TOP_OUT=1;
    __delay_us(12);
    US_TOP_OUT=0;

    WRITETIMER1(0);
    while(US_TOP_IN==0 && PIR1bits.TMR1IF==0);
    WRITETIMER1(0);
    while(US_TOP_IN==1 && PIR1bits.TMR1IF==0);
    time_us=READTIMER1();
    return (time_us/928); //returns value in cm
}

/******************************************************************************
//------------------------------------------------------------------------------
//******************************************************************************/

void ReadServo(char* buffer, unsigned char len)
{
  char i;    // Length counter
  unsigned char data;

  for(i=0;i<len;i++)  // Only retrieve len characters
  {
   while(PIR3bits.RC2IF == 0);// Wait for data to be received

 //   data = getc2USART();    // Get a character from the USART
                           // and save in the jump_string
   data = RCREG2;

   *buffer = data;
    buffer++;              // Increment the jump_string pointer
  }
}

void RemoteControl()
{
   unsigned long num;
   char j,jj;
   LCDinit();
   while(1)
   {
        num=atol(UART_RX_Buffer);

        Battery_voltage=((num/10000)*15260)/1000;
        Current=((num%10000)*854)/1000;
        //*****
        j=PrintText(1,20,"REMOTE ");
        PrintText(1,j,"CONTROL");

        j=PrintText(7,1,"UART:");
        PrintNumber(7,j,UARTcycle);

        switch (motorGo)
        {
            case Ureverse:
                PrintText(7,65,"reverse  ");break;
            case Uforward:
                PrintText(7,65,"forward  ");break;
            case UStop_turning:
            case UStop_drive:
                PrintText(7,65,"Stop      ");break;
            case UTurn_right:
                PrintText(7,65,"right     ");break;
            case UTurn_left:
                PrintText(7,65,"left        ");break;
        }

        j=PrintText(8,1,"VBat: ");
        jj=PrintNumber(8,j,Battery_voltage);
        PrintText(8,jj,"mV     ");

        j=PrintText(8,75,"I: ");
        jj=PrintNumber(8,j,Current);
        PrintText(8,jj,"mA   ");
   }
}

void Window()
{
    char j,jj;

    PutCharMys(5,20, 1+(('0'-1)*!RX0));
    PutCharMys(5,30, 1+(('0'-1)*!RX1));
    PutCharMys(5,36, 1+(('0'-1)*!RX2));
    PutCharMys(5,42, 1+(('0'-1)*!RX3));
    PutCharMys(5,48, 1+(('0'-1)*!RX4));


    PrintNumber(7,35,Get_US_FWD());
    PrintNumber(7,90, Get_US_TOP());

    if(Z[ZmotorGo] != motorGo) {
    switch(motorGo)
    {
        case Uforward:    PrintText(5,110, " \02 "); break;
        case Ureverse:    PrintText(5,110, " \03  "); break;
        case UTurn_left:  PrintText(5,110, " \04  "); break;
        case UTurn_right: PrintText(5,110, " \05  "); break;
        case UStop_drive: PrintText(5,110, " 0 "); break;
    }}

    if(Z[Zenc2_bool] != ENC2)
    {EncoderShort++; EncoderTotal++;
    PrintNumber(4,50,EncoderShort);
    PrintNumber(4,95,EncoderTotal); }

    Z[ZmotorGo] = motorGo;
    Z[Zenc2] = EncoderTotal;
    Z[Zenc2_bool]=ENC2;
}

void WindowForm(unsigned char* Name)
{
    //char* jump_string;
    LCDinit();
    PrintText(1,30,Name);

    PrintText(3,1,"Position:");

    PrintText(4,1 ,"encoder:");


    PrintText(5,1, "rx:");
    PrintText(5,66, "motors: ");

    PrintText(7,1, "us\02:");
    PrintText(7,66,"us\03:");
    PrintText(7,113,"cm");


    PrintText(8,1,"bat:");
    PrintNumber(8,20,Battery_voltage);
    PrintText(8,48,"mv");

    PrintText(8,66,"I: ");
    PrintNumber(8,75,Current);
    PrintText(8,111,"mA");

    PutCharMys(5,20, 1+(('0'-1)*!RX0));
    PutCharMys(5,30, 1+(('0'-1)*!RX1));
    PutCharMys(5,36, 1+(('0'-1)*!RX2));
    PutCharMys(5,42, 1+(('0'-1)*!RX3));
    PutCharMys(5,48, 1+(('0'-1)*!RX4));
}


void GoStraight(char where)
{
    PrintNumber(2,80,++part);
    Window();
    PrintText(2,1,"FRONT     ");
    if(where>0)  Motor_control_forward
    if(where==0) Motor_control_reverse

    EncoderShort=0;
    while((EncoderShort)<(6-(half*2))) Window();
    while((RX0 + RX1 + RX2 + RX3 + RX4)<(3-edge)) Window();
    Motor_control_Stop; Window();  Wait(wait_line,s);

    Align(0);

    Window();
    Motor_control_Stop
    Wait(wait_line,s);
    half=false;
    PrintText(2,1,"                        ");
}

void GoStraightB(char nothing)
{
    PrintNumber(2,80,++part);
    PrintText(2,1,"BACK     ");
    Window();
    Motor_control_reverse

    EncoderShort=0;
    while((EncoderShort)<(6*(!half))) Window();
    while((RX0 + RX1 + RX2 + RX3 + RX4)<(3-edge)) Window();
    Motor_control_Stop; Window();  Wait(wait_line,s);

    Align(1);

    Window();
    Motor_control_Stop
    Wait(wait_line,s);
    half=false;
    PrintText(2,1,"                        ");
}

void GoStraightHalf(char where, short minus)
{
    PrintNumber(2,80,++part);
   PrintText(2,1,"HALF                ");
   Window();

   if(where>0)  Motor_control_forward
   if(where==0) Motor_control_reverse

   EncoderShort=0;
   while((EncoderShort)<(9-minus)) Window();

   Window();
   Motor_control_Stop
   Wait(wait_line,s);
    PrintText(2,1,"                        ");
}

void TurnRightAngle(char where)
{
    PrintNumber(2,80,++part);
    PrintText(2,1,"TURN                 ");

   if(where=='<') {Motor_control_TurnLeft }
   if(where=='>') {Motor_control_TurnRight}

   EncoderShort=0;
   while((EncoderShort)<14) Window();

   Window();
   Motor_control_Stop
   Wait(wait_line,s);

   if(half==false)
   {
     Motor_control_forward
     EncoderShort=0;
     while((RX0 + RX1 + RX2 + RX3 + RX4)<(3-edge) && (EncoderShort)<5) Window();
     Motor_control_Stop; Window(); Wait(wait_line,s);

     if(EncoderShort<5)
     {
       Align(0);
       return;
     }

     Motor_control_reverse
     EncoderShort=0;
     while((RX0 + RX1 + RX2 + RX3 + RX4)<(3-edge) && (EncoderShort)<10) Window();
     Motor_control_Stop; Window(); Wait(wait_line,s);

     Align(1);
    }

     Window();
     Motor_control_Stop
     Wait(wait_line,s);
     PrintText(2,1,"                        ");
}


void Align(bool vice_versa)
{
    if(vice_versa==true)
    {
        EncoderShort=0;
        if(RX4==not_black)  {PrintText(2,50,"RX4      ");Motor_control_TurnRight; while((RX4==not_black) && EncoderShort<=2)  Window();
if(RX4==not_black)PrintText(2,50,"RX0cor   ");Motor_control_TurnLeft;  while((RX0==not_black) && EncoderShort<=4)  Window();}
        EncoderShort=0;
        if(RX0==not_black)  {PrintText(2,50,"RX0      ");Motor_control_TurnLeft;  while((RX0==not_black) && EncoderShort<=2)  Window();
if(RX0==not_black) PrintText(2,50,"RX4cor   ");Motor_control_TurnRight; while((RX4==not_black) && EncoderShort<=4)  Window();}
         PrintText(2,50,"              ");
         return;
    }

    else
    {
         EncoderShort=0;
        if(RX0==not_black)  {PrintText(2,50,"RX0      ");Motor_control_TurnRight; while(RX0==not_black && EncoderShort<=2)  Window();
if(RX0==not_black) PrintText(2,50,"RX4cor   ");Motor_control_TurnLeft;  while(RX4==not_black && EncoderShort<=4)  Window();}
        EncoderShort=0;
        if(RX4==not_black)  {PrintText(2,50,"RX4      ");Motor_control_TurnLeft;  while(RX4==not_black && EncoderShort<=2)  Window();
if(RX4==not_black) PrintText(2,50,"RX0cor   ");Motor_control_TurnRight; while(RX0==not_black && EncoderShort<=4)  Window();}
         PrintText(2,50,"              ");

    }




}