File content as of revision 1:202a0e209b73:

#include "mbed.h"
#include "Servo.h"
//CONNCTION OF PIXY WITH FRDM
#define mosiPin PTD3
#define misoPin PTD2
#define clockPin PTD1
#define sspin PTD0

#define speed_Limit 10

#define PIXY_SPI_CLOCKRATE       2000000


//first bytes of pixy 2 packets with/without checksum packets;
#define hex_no_Checksum_First_Byte 0xAE
#define hex_no_Checksum_Second_Byte 0xC1
#define hex_Checksum_First_Byte 0xAF
//type
#define hex_Set_Lamp_Request 0X16//set Lamp request
#define hex_Get_Main_Features_Request 0X30//Get Main_Features request
#define hex_Get_Main_Features_Response 0X31//Get Main_Features response
#define hex_Set_Mode_Request 0X36//Set Mode request
#define hex_Set_Next_Turn_Request 0X3A//Set Next Turn request
#define hex_Set_Default_Turn_Request 0X3C//Set Default Turn request
#define hex_Set_Vector_Request 0X38//Set Vector request
#define hex_Revese_Vector_Request 0X3E//Reverse Vector request
#define PIXY_TYPE_REQUEST_CHANGE_PROG 0x02//request program change

#define PIXY_MAX_PROGNAME 0x21//change programe
#define LINE_MAX_INTERSECTION_LINES 30
//the Feature is set with an or request -->3->Vectors+ Intersection
#define FeatureVector 1
#define FeatureIntersection 2
#define FeatureBarcode 4

struct Vector {


    uint8_t m_x0;
    uint8_t m_y0;
    uint8_t m_x1;
    uint8_t m_y1;
    uint8_t m_index;
    uint8_t m_flags;
};

struct IntersectionLine {
    uint8_t m_index;
    uint8_t m_reserved;
    int16_t m_angle;
};

struct Intersection {
    void print()
    {
        char buf[64];
        uint8_t i;
        sprintf(buf, "intersection: (%d %d)", m_x, m_y);
        printf("%s\n", buf);
        for (i = 0; i < m_n; i++) {
            sprintf(buf, "  %d: index: %d angle: %d", i, m_intLines[i].m_index, m_intLines[i].m_angle);
            printf("%s\n", buf);
        }
    }

    uint8_t m_x;
    uint8_t m_y;

    uint8_t m_n;
    uint8_t m_reserved;
    IntersectionLine m_intLines[LINE_MAX_INTERSECTION_LINES];
};

struct Barcode {
    void print()
    {
        char buf[64];
        sprintf(buf, "Barcode: (%d %d), val: %d flags: %d", m_x, m_y, m_code, m_flags);
        printf("%s\n", buf);
    }

    uint8_t m_x;
    uint8_t m_y;
    uint8_t m_flags;
    uint8_t m_code;
};

uint8_t Buffer[40];
Vector vectors[20];
Intersection intersections[10];
Barcode barcodes[10];
int length = 0;
int numberOfVector = 0;
int numberOfIntersections = 0;
int numberOfbarcodes = 0;


double angle = 0;
int biggest_nb;
int biggest_y;
int speed;


SPI spi(mosiPin, misoPin, clockPin);//mosi,miso,clock
DigitalOut ssPin(sspin, 1);//pin for slave selection

Servo motorLeft(PTE20);
Servo motorRight(PTE21);
Servo myservo(PTE29);



bool cheksumValidation(uint8_t *Buff, uint8_t len)
{
    if (len < 6) {
        printf("Len <6");
        return false;
    }
    uint16_t Checksum = Buff[5] << 8 | Buff[4];
    uint16_t sum = 0x0000;
    for (int i = 6; i < len; i++)
        sum += Buff[i];
    return sum == Checksum;

}

void clearBuffer(uint8_t *buff)
{
    for (int i = 0; i < 40; i++)
        buff[i] = 0x00;
}

int16_t recv(uint8_t *buf)
{
    uint16_t *cs = NULL;
    if (cs)
        *cs = 0;
    uint8_t len;
    uint8_t i;
    ssPin = 0;
    bool frst = false;
    for (i = 0; i < 4; i++) {
        if (i == 0)
            do {
                buf[i] = spi.write(0x00);

                if (cs)
                    *cs += buf[i];
                if (buf[i] == 0xaf)
                    frst = true;


            } while (!frst);
        else {
            buf[i] = spi.write(0x00);

            if (cs)
                *cs += buf[i];

        }
    }
    if (buf[0] == hex_Checksum_First_Byte)
        len = 2 + buf[3];
    else len = buf[3];

    for (i = 0; i < 35; i++) {
        buf[i + 4] = spi.write(0x00);
    }
    len += 4;
    ssPin = 1;
    return len;
}

int16_t send(uint8_t *buf, uint8_t len)
{
    uint8_t i;
    ssPin = 0;
    for (i = 0; i < len; i++)
        spi.write(buf[i]);
    ssPin = 1;
    return len;
}

void getMainFeaturesHandler(uint8_t *buf, uint8_t len)
{
    numberOfVector = 0;
    numberOfbarcodes = 0;
    numberOfIntersections = 0;
    printf("CHecking Features\n");
    for (int i = 6; i < len; i++) {
        switch (buf[i]) {
            case FeatureVector:
                ++i;
                printf("found vector\n");
                vectors[numberOfVector].m_x0 = buf[++i];
                vectors[numberOfVector].m_y0 = buf[++i];
                vectors[numberOfVector].m_x1 = buf[++i];
                vectors[numberOfVector].m_y1 = buf[++i];
                vectors[numberOfVector].m_index = buf[++i];
                vectors[numberOfVector].m_flags = buf[++i];
                numberOfVector++;
                break;
            case FeatureIntersection:
                ++i;
                intersections[numberOfIntersections].m_x = buf[++i];
                intersections[numberOfIntersections].m_y = buf[++i];
                intersections[numberOfIntersections].m_n = buf[++i];
                intersections[numberOfIntersections].m_reserved = buf[++i];
                for (int j = 0; j < LINE_MAX_INTERSECTION_LINES; j++) {
                    intersections[numberOfIntersections].m_intLines->m_index = buf[++i];
                    intersections[numberOfIntersections].m_intLines->m_reserved = buf[++i];
                    intersections[numberOfIntersections].m_intLines->m_angle = buf[++i];
                }
                break;
            case FeatureBarcode:
                i++;
                barcodes[numberOfbarcodes].m_x = buf[++i];
                barcodes[numberOfbarcodes].m_y = buf[++i];
                barcodes[numberOfbarcodes].m_flags = buf[++i];
                barcodes[numberOfbarcodes].m_code = buf[++i];
                break;
        }
    }
}

void setLamp(uint8_t value1, uint8_t value2)
{
    clearBuffer(Buffer);
    Buffer[0] = hex_no_Checksum_First_Byte;
    Buffer[1] = hex_no_Checksum_Second_Byte;
    Buffer[2] = hex_Set_Lamp_Request;
    Buffer[3] = 0x02;
    Buffer[4] = value1;
    Buffer[5] = value2;
    printf("V1:%d,V2:%d\n\r", value1, value2);
    int16_t len = send(Buffer, 6);
    len = recv(Buffer);

}

void getFeatures(uint8_t type, uint8_t Features)
{

    clearBuffer(Buffer);
    Buffer[0] = hex_no_Checksum_First_Byte;
    Buffer[1] = hex_no_Checksum_Second_Byte;
    Buffer[2] = hex_Get_Main_Features_Request;
    Buffer[3] = 0x02;
    Buffer[4] = type;
    Buffer[5] = Features;

    int16_t len = send(Buffer, 6);
    printf("Getting features\n");

    len = recv(Buffer);
    getMainFeaturesHandler(Buffer, len);
}

void changeProg(const char *prog)
{
    clearBuffer(Buffer);
    Buffer[0] = hex_no_Checksum_First_Byte;
    Buffer[1] = hex_no_Checksum_Second_Byte;
    Buffer[2] = PIXY_TYPE_REQUEST_CHANGE_PROG;
    Buffer[3] = strlen(prog);
    length = send(Buffer, Buffer[3] + 4);
    length = recv(Buffer);

}



void Print(int i)
{
    char buf[64];
    sprintf(buf, "vector: (%d %d) (%d %d) index: %d flags %d", vectors[i].m_x0, vectors[i].m_y0, vectors[i].m_x1,
            vectors[i].m_y1, vectors[i].m_index, vectors[i].m_flags);
    printf("%s\n", buf);
}

void setSpeed(int speed)
{
    printf("CHanged Speed\n");
    double var=speed*speed_Limit/100;
    var = var/100 +0.5;
    motorLeft = var;
    motorRight = var;
}

void arm()//arm the esc
{
    motorLeft = 0.0;
    motorRight = 0.0;
    wait(0.5); //detects signal
    //Required ESC Calibration/Arming sequence
    //sends longest and shortest PWM pulse to learn and arm at power on
    motorLeft = 1.0; //send longest PWM
    motorRight = 1.0;
    wait(2);
    motorLeft = 0.0; //send shortest PWM
    motorRight = 0.0;
    wait(2);
//ESC now operational using standard servo PWM signals
    for (float p = 0; p <= 0.5; p += 0.025) {
        printf("Arming underway\n");
        motorLeft = p;
        motorRight = p;
        wait_ms(100);
    


    }
     printf("leaving arming");   
    
}

void loop()
{

    //Serial.println(angle);

    int8_t i;
    getFeatures(0x01, 0x07);

    if (numberOfVector > 0) {
        biggest_nb = 0;
        biggest_y = vectors[0].m_y1 - vectors[0].m_y0;
        for (i = 0; i < numberOfVector; i++) {
            if ((vectors[i].m_y1 - vectors[i].m_y0) > biggest_y) {
                biggest_y = vectors[i].m_y1 - vectors[i].m_y0;
                biggest_nb = i;
            }
        }


        //    for (i = 0; i < numberOfVector; i++)
        //    {
        Print(biggest_nb);
        int x0 = vectors[biggest_nb].m_x0;
        int x1 = vectors[biggest_nb].m_x1;
        int x = x0 - x1;
        int y0 = vectors[biggest_nb].m_y0;
        int y1 = vectors[biggest_nb].m_y1;
        int y = y0 - y1;
        if (y < 0)y *= -1;
        double param= y / x;
        angle = atan(param) * 180/3.14;
        printf("angle:%d\n", angle);


        if (angle > -40 || angle < 40) {
            myservo.position(angle);
            setSpeed(30);
        } else {
            myservo.position(0);
            setSpeed(50);
        }
        wait_ms(100);
    } else {
        myservo.position(angle);
        setSpeed(0);
        wait_ms(50);
    }

}




/*******************************start part for jihad*******************************/
bool Part1=false;bool cou3_3al_yamin=false;
bool Part2=false;bool cou3_3al_chmel=false;
bool msalbiye_detected=false;
bool the_end=false;
void check_the_end();
void rouh_chmel();
void rouh_yamin();
void cawi3_yamin();
void cawi3_chmel();
void Check_msalbiye();
void Check_vector_left(Vector v);
void Check_vector_right(Vector v);
void check_lines();
void J_loop();
void Check_intersections();
void jihad_tester_loop();
void Check_cwe3(Vector v);
void wait_with_jihad(double i);
bool jihad_timer_s=false;
Ticker tick;
Timer t;

/**********************************end part for jihad******************************/




int main()
{
    spi.format(8, 0);
    spi.frequency(PIXY_SPI_CLOCKRATE);
    myservo.calibrate(0.001, 90);
    myservo.write(0.5);
    //call the esc arming function
    arm();
  //  while(1)
//{    motorLeft =0.8;
 //   motorRight =0.8;}
    //printf("process started");
//    setSpeed(100);
    //printf("process started");
   changeProg("line");
    printf("process started");
    //setSpeed(50);
    jihad_tester_loop();
    
//printf("process started");
 //  J_loop();
//while(1){
//     
//     myservo.position(40);
//     wait(3);
//      myservo.position(-40);
//      wait(3);
//       myservo.position(0);
//        wait(3);
//    
//    }

}



/*******************************start AI of jihad*******************************/
void wait_with_jihad_s(double i){
    jihad_time=true;
    int j=0;
    i*=1000;
    while(jihad_timer){
        wait_ms(1);
        j++;
        if(j>=i)jihad_timer=false;
        }
    }
void jihad_tester_loop(){
    setSpeed(60);
    while (1) {
        int8_t i;
        getFeatures(0x01, 0x07);
        for (int i = 0; i < numberOfVector; i++) {
            Check_cwe3(vectors[i]);
            }
        if(!cou3_3al_chmel&&!cou3_3al_yamin)
            myservo.position(0);
        if(cou3_3al_chmel)
            cawi3_chmel();
        if(cou3_3al_yamin)
            cawi3_yamin();
        
        //wait_ms(50);
        cou3_3al_chmel=false;
        cou3_3al_yamin=false;   
        Part1=false;
        Part2=false;
        }
    }
//myservo.position(0);
    
    
    
    
    

void Check_cwe3(Vector v){
     if(v.m_y0<35&&v.m_y1>15&&v.m_x0<35&&v.m_x1>35)
        cou3_3al_chmel=true;
     if(v.m_y1<35&&v.m_y0>15&&v.m_x1<35&&v.m_x0>35)
        cou3_3al_chmel=true;
     if(v.m_y0<35&&v.m_y1>15&&v.m_x0>35&&v.m_x1<35)
        cou3_3al_yamin=true;
     if(v.m_y1<35&&v.m_y0>15&&v.m_x1>35&&v.m_x0<35)
        cou3_3al_yamin=true;
    
    
    }
void check_the_end(){
        int three_nb=0;
        for(int i = 0; i < numberOfVector; i++) {
            if ( (vectors[i].m_x0<65&&vectors[i].m_x0>10)||(vectors[i].m_x1<65&&vectors[i].m_x1>10))
                if ( (vectors[i].m_y0>10&&vectors[i].m_y0<20)||(vectors[i].m_y1>10&&vectors[i].m_y1<20))
                    three_nb++;
            }
        if(three_nb>=3){
            wait_ms(500);
            the_end=true;
            setSpeed(0);
            }
    }
void rouh_chmel(){
    myservo.position(-45);
    
    wait_ms(50);
    //myservo.position(0);
    }

void rouh_yamin(){
    myservo.position(45);
    wait_ms(50);
    //myservo.position(0);
    }
void cawi3_yamin(){
    myservo.position(40);
    
//   wait_ms(1000);
  //  myservo.position(0);
    }
     
void cawi3_chmel(){
    myservo.position(-40);
//    wait_ms(1000);
  // wait(1.5);
//    myservo.position(0);
    }

void Check_msalbiye(){
     if(numberOfIntersections>=4){
        Part1=true;
        Part2=true;
        wait(1);}
    }


void Check_intersections(){
    bool cou3_yamin=false;
    bool cou3_chmel=false;
    bool mata3mel_chi=false;
        for(int i=0;i<numberOfIntersections;i++){
            if(intersections[i].m_y<15)
               if(intersections[i].m_x<20)
                    cou3_yamin=true;
             if(intersections[i].m_y<15)
               if(intersections[i].m_x>68)
                    cou3_chmel=true;
            }
    if(cou3_chmel&&cou3_yamin)
        mata3mel_chi=true;
    if(!mata3mel_chi){
        if(cou3_chmel)
            cawi3_chmel();
        if(cou3_yamin)
            cawi3_yamin();
        }
    }



void Check_vector_left(Vector v){
    if(v.m_y0<16&&v.m_y1>45&&v.m_x0<35&&v.m_x1<35)
        Part1=true;
    if(v.m_y1<16&&v.m_y0>45&&v.m_x0<35&&v.m_x1<35)
        Part1=true;
    }
    
void Check_vector_right(Vector v){
    printf("check_vect_right");
    if(v.m_y0<16&&v.m_y1>30&&v.m_x0>35&&v.m_x1>35)
        Part2=true;
    if(v.m_y1<16&&v.m_y0>30&&v.m_x0>35&&v.m_x1>35)
        Part2=true;
    }

void check_lines(){
    
    for (int i = 0; i < numberOfVector; i++) {
            Print(i);
            Check_vector_left(vectors[i]);
            Check_vector_right(vectors[i]);
        }
    }

void J_loop(){
    //motorLeft =0.8;
    //motorRight =0.8;
   // printf("entering_jihad loop");
    setSpeed(50);
    while(!the_end){
        int8_t i;
        getFeatures(0x01, 0x07);
        
        //if(numberOfVector >= 5)
           // check_the_end();
        
        
        if (numberOfVector > 0) {
            check_lines();
            }
       // Check_msalbiye();    
//        if(!Part1)
//            rouh_chmel();
//        if(!Part2)
//            rouh_yamin();
        
        if(Part2&&Part1)
            myservo.position(0);
        
          
        // if(numberOfIntersections>=2)
        //      Check_intersections(); 
            
        Part1=false;
        Part2=false;
        }
    //setSpeed(0);
    }