main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 //CONNCTION OF PIXY WITH FRDM
00004 #define mosiPin PTD3
00005 #define misoPin PTD2
00006 #define clockPin PTD1
00007 #define sspin PTD0
00008 
00009 #define speed_Limit 10
00010 
00011 #define PIXY_SPI_CLOCKRATE       2000000
00012 
00013 
00014 //first bytes of pixy 2 packets with/without checksum packets;
00015 #define hex_no_Checksum_First_Byte 0xAE
00016 #define hex_no_Checksum_Second_Byte 0xC1
00017 #define hex_Checksum_First_Byte 0xAF
00018 //type
00019 #define hex_Set_Lamp_Request 0X16//set Lamp request
00020 #define hex_Get_Main_Features_Request 0X30//Get Main_Features request
00021 #define hex_Get_Main_Features_Response 0X31//Get Main_Features response
00022 #define hex_Set_Mode_Request 0X36//Set Mode request
00023 #define hex_Set_Next_Turn_Request 0X3A//Set Next Turn request
00024 #define hex_Set_Default_Turn_Request 0X3C//Set Default Turn request
00025 #define hex_Set_Vector_Request 0X38//Set Vector request
00026 #define hex_Revese_Vector_Request 0X3E//Reverse Vector request
00027 #define PIXY_TYPE_REQUEST_CHANGE_PROG 0x02//request program change
00028 
00029 #define PIXY_MAX_PROGNAME 0x21//change programe
00030 #define LINE_MAX_INTERSECTION_LINES 30
00031 //the Feature is set with an or request -->3->Vectors+ Intersection
00032 #define FeatureVector 1
00033 #define FeatureIntersection 2
00034 #define FeatureBarcode 4
00035 
00036 struct Vector {
00037 
00038 
00039     uint8_t m_x0;
00040     uint8_t m_y0;
00041     uint8_t m_x1;
00042     uint8_t m_y1;
00043     uint8_t m_index;
00044     uint8_t m_flags;
00045 };
00046 
00047 struct IntersectionLine {
00048     uint8_t m_index;
00049     uint8_t m_reserved;
00050     int16_t m_angle;
00051 };
00052 
00053 struct Intersection {
00054     void print()
00055     {
00056         char buf[64];
00057         uint8_t i;
00058         sprintf(buf, "intersection: (%d %d)", m_x, m_y);
00059         printf("%s\n", buf);
00060         for (i = 0; i < m_n; i++) {
00061             sprintf(buf, "  %d: index: %d angle: %d", i, m_intLines[i].m_index, m_intLines[i].m_angle);
00062             printf("%s\n", buf);
00063         }
00064     }
00065 
00066     uint8_t m_x;
00067     uint8_t m_y;
00068 
00069     uint8_t m_n;
00070     uint8_t m_reserved;
00071     IntersectionLine m_intLines[LINE_MAX_INTERSECTION_LINES];
00072 };
00073 
00074 struct Barcode {
00075     void print()
00076     {
00077         char buf[64];
00078         sprintf(buf, "Barcode: (%d %d), val: %d flags: %d", m_x, m_y, m_code, m_flags);
00079         printf("%s\n", buf);
00080     }
00081 
00082     uint8_t m_x;
00083     uint8_t m_y;
00084     uint8_t m_flags;
00085     uint8_t m_code;
00086 };
00087 
00088 uint8_t Buffer[40];
00089 Vector vectors[20];
00090 Intersection intersections[10];
00091 Barcode barcodes[10];
00092 int length = 0;
00093 int numberOfVector = 0;
00094 int numberOfIntersections = 0;
00095 int numberOfbarcodes = 0;
00096 
00097 
00098 double angle = 0;
00099 int biggest_nb;
00100 int biggest_y;
00101 int speed;
00102 
00103 
00104 SPI spi(mosiPin, misoPin, clockPin);//mosi,miso,clock
00105 DigitalOut ssPin(sspin, 1);//pin for slave selection
00106 
00107 Servo motorLeft(PTE20);
00108 Servo motorRight(PTE21);
00109 Servo myservo(PTE29);
00110 
00111 
00112 
00113 bool cheksumValidation(uint8_t *Buff, uint8_t len)
00114 {
00115     if (len < 6) {
00116         printf("Len <6");
00117         return false;
00118     }
00119     uint16_t Checksum = Buff[5] << 8 | Buff[4];
00120     uint16_t sum = 0x0000;
00121     for (int i = 6; i < len; i++)
00122         sum += Buff[i];
00123     return sum == Checksum;
00124 
00125 }
00126 
00127 void clearBuffer(uint8_t *buff)
00128 {
00129     for (int i = 0; i < 40; i++)
00130         buff[i] = 0x00;
00131 }
00132 
00133 int16_t recv(uint8_t *buf)
00134 {
00135     uint16_t *cs = NULL;
00136     if (cs)
00137         *cs = 0;
00138     uint8_t len;
00139     uint8_t i;
00140     ssPin = 0;
00141     bool frst = false;
00142     for (i = 0; i < 4; i++) {
00143         if (i == 0)
00144             do {
00145                 buf[i] = spi.write(0x00);
00146 
00147                 if (cs)
00148                     *cs += buf[i];
00149                 if (buf[i] == 0xaf)
00150                     frst = true;
00151 
00152 
00153             } while (!frst);
00154         else {
00155             buf[i] = spi.write(0x00);
00156 
00157             if (cs)
00158                 *cs += buf[i];
00159 
00160         }
00161     }
00162     if (buf[0] == hex_Checksum_First_Byte)
00163         len = 2 + buf[3];
00164     else len = buf[3];
00165 
00166     for (i = 0; i < 35; i++) {
00167         buf[i + 4] = spi.write(0x00);
00168     }
00169     len += 4;
00170     ssPin = 1;
00171     return len;
00172 }
00173 
00174 int16_t send(uint8_t *buf, uint8_t len)
00175 {
00176     uint8_t i;
00177     ssPin = 0;
00178     for (i = 0; i < len; i++)
00179         spi.write(buf[i]);
00180     ssPin = 1;
00181     return len;
00182 }
00183 
00184 void getMainFeaturesHandler(uint8_t *buf, uint8_t len)
00185 {
00186     numberOfVector = 0;
00187     numberOfbarcodes = 0;
00188     numberOfIntersections = 0;
00189     printf("CHecking Features\n");
00190     for (int i = 6; i < len; i++) {
00191         switch (buf[i]) {
00192             case FeatureVector:
00193                 ++i;
00194                 printf("found vector\n");
00195                 vectors[numberOfVector].m_x0 = buf[++i];
00196                 vectors[numberOfVector].m_y0 = buf[++i];
00197                 vectors[numberOfVector].m_x1 = buf[++i];
00198                 vectors[numberOfVector].m_y1 = buf[++i];
00199                 vectors[numberOfVector].m_index = buf[++i];
00200                 vectors[numberOfVector].m_flags = buf[++i];
00201                 numberOfVector++;
00202                 break;
00203             case FeatureIntersection:
00204                 ++i;
00205                 intersections[numberOfIntersections].m_x = buf[++i];
00206                 intersections[numberOfIntersections].m_y = buf[++i];
00207                 intersections[numberOfIntersections].m_n = buf[++i];
00208                 intersections[numberOfIntersections].m_reserved = buf[++i];
00209                 for (int j = 0; j < LINE_MAX_INTERSECTION_LINES; j++) {
00210                     intersections[numberOfIntersections].m_intLines->m_index = buf[++i];
00211                     intersections[numberOfIntersections].m_intLines->m_reserved = buf[++i];
00212                     intersections[numberOfIntersections].m_intLines->m_angle = buf[++i];
00213                 }
00214                 break;
00215             case FeatureBarcode:
00216                 i++;
00217                 barcodes[numberOfbarcodes].m_x = buf[++i];
00218                 barcodes[numberOfbarcodes].m_y = buf[++i];
00219                 barcodes[numberOfbarcodes].m_flags = buf[++i];
00220                 barcodes[numberOfbarcodes].m_code = buf[++i];
00221                 break;
00222         }
00223     }
00224 }
00225 
00226 void setLamp(uint8_t value1, uint8_t value2)
00227 {
00228     clearBuffer(Buffer);
00229     Buffer[0] = hex_no_Checksum_First_Byte;
00230     Buffer[1] = hex_no_Checksum_Second_Byte;
00231     Buffer[2] = hex_Set_Lamp_Request;
00232     Buffer[3] = 0x02;
00233     Buffer[4] = value1;
00234     Buffer[5] = value2;
00235     printf("V1:%d,V2:%d\n\r", value1, value2);
00236     int16_t len = send(Buffer, 6);
00237     len = recv(Buffer);
00238 
00239 }
00240 
00241 void getFeatures(uint8_t type, uint8_t Features)
00242 {
00243 
00244     clearBuffer(Buffer);
00245     Buffer[0] = hex_no_Checksum_First_Byte;
00246     Buffer[1] = hex_no_Checksum_Second_Byte;
00247     Buffer[2] = hex_Get_Main_Features_Request;
00248     Buffer[3] = 0x02;
00249     Buffer[4] = type;
00250     Buffer[5] = Features;
00251 
00252     int16_t len = send(Buffer, 6);
00253     printf("Getting features\n");
00254 
00255     len = recv(Buffer);
00256     getMainFeaturesHandler(Buffer, len);
00257 }
00258 
00259 void changeProg(const char *prog)
00260 {
00261     clearBuffer(Buffer);
00262     Buffer[0] = hex_no_Checksum_First_Byte;
00263     Buffer[1] = hex_no_Checksum_Second_Byte;
00264     Buffer[2] = PIXY_TYPE_REQUEST_CHANGE_PROG;
00265     Buffer[3] = strlen(prog);
00266     length = send(Buffer, Buffer[3] + 4);
00267     length = recv(Buffer);
00268 
00269 }
00270 
00271 
00272 
00273 void Print(int i)
00274 {
00275     char buf[64];
00276     sprintf(buf, "vector: (%d %d) (%d %d) index: %d flags %d", vectors[i].m_x0, vectors[i].m_y0, vectors[i].m_x1,
00277             vectors[i].m_y1, vectors[i].m_index, vectors[i].m_flags);
00278     printf("%s\n", buf);
00279 }
00280 
00281 void setSpeed(int speed)
00282 {
00283     printf("CHanged Speed\n");
00284     double var=speed*speed_Limit/100;
00285     var = var/100 +0.5;
00286     motorLeft = var;
00287     motorRight = var;
00288 }
00289 
00290 void arm()//arm the esc
00291 {
00292     motorLeft = 0.0;
00293     motorRight = 0.0;
00294     wait(0.5); //detects signal
00295     //Required ESC Calibration/Arming sequence
00296     //sends longest and shortest PWM pulse to learn and arm at power on
00297     motorLeft = 1.0; //send longest PWM
00298     motorRight = 1.0;
00299     wait(2);
00300     motorLeft = 0.0; //send shortest PWM
00301     motorRight = 0.0;
00302     wait(2);
00303 //ESC now operational using standard servo PWM signals
00304     for (float p = 0; p <= 0.5; p += 0.025) {
00305         printf("Arming underway\n");
00306         motorLeft = p;
00307         motorRight = p;
00308         wait_ms(100);
00309     
00310 
00311 
00312     }
00313      printf("leaving arming");   
00314     
00315 }
00316 
00317 void loop()
00318 {
00319 
00320     //Serial.println(angle);
00321 
00322     int8_t i;
00323     getFeatures(0x01, 0x07);
00324 
00325     if (numberOfVector > 0) {
00326         biggest_nb = 0;
00327         biggest_y = vectors[0].m_y1 - vectors[0].m_y0;
00328         for (i = 0; i < numberOfVector; i++) {
00329             if ((vectors[i].m_y1 - vectors[i].m_y0) > biggest_y) {
00330                 biggest_y = vectors[i].m_y1 - vectors[i].m_y0;
00331                 biggest_nb = i;
00332             }
00333         }
00334 
00335 
00336         //    for (i = 0; i < numberOfVector; i++)
00337         //    {
00338         Print(biggest_nb);
00339         int x0 = vectors[biggest_nb].m_x0;
00340         int x1 = vectors[biggest_nb].m_x1;
00341         int x = x0 - x1;
00342         int y0 = vectors[biggest_nb].m_y0;
00343         int y1 = vectors[biggest_nb].m_y1;
00344         int y = y0 - y1;
00345         if (y < 0)y *= -1;
00346         double param= y / x;
00347         angle = atan(param) * 180/3.14;
00348         printf("angle:%d\n", angle);
00349 
00350 
00351         if (angle > -40 || angle < 40) {
00352             myservo.position(angle);
00353             setSpeed(30);
00354         } else {
00355             myservo.position(0);
00356             setSpeed(50);
00357         }
00358         wait_ms(100);
00359     } else {
00360         myservo.position(angle);
00361         setSpeed(0);
00362         wait_ms(50);
00363     }
00364 
00365 }
00366 
00367 
00368 
00369 
00370 /*******************************start part for jihad*******************************/
00371 bool Part1=false;bool cou3_3al_yamin=false;
00372 bool Part2=false;bool cou3_3al_chmel=false;
00373 bool msalbiye_detected=false;
00374 bool the_end=false;
00375 void check_the_end();
00376 void rouh_chmel();
00377 void rouh_yamin();
00378 void cawi3_yamin();
00379 void cawi3_chmel();
00380 void Check_msalbiye();
00381 void Check_vector_left(Vector v);
00382 void Check_vector_right(Vector v);
00383 void check_lines();
00384 void J_loop();
00385 void Check_intersections();
00386 void jihad_tester_loop();
00387 void Check_cwe3(Vector v);
00388 void wait_with_jihad(double i);
00389 bool jihad_timer_s=false;
00390 Ticker tick;
00391 Timer t;
00392 
00393 /**********************************end part for jihad******************************/
00394 
00395 
00396 
00397 
00398 int main()
00399 {
00400     spi.format(8, 0);
00401     spi.frequency(PIXY_SPI_CLOCKRATE);
00402     myservo.calibrate(0.001, 90);
00403     myservo.write(0.5);
00404     //call the esc arming function
00405     arm();
00406   //  while(1)
00407 //{    motorLeft =0.8;
00408  //   motorRight =0.8;}
00409     //printf("process started");
00410 //    setSpeed(100);
00411     //printf("process started");
00412    changeProg("line");
00413     printf("process started");
00414     //setSpeed(50);
00415     jihad_tester_loop();
00416     
00417 //printf("process started");
00418  //  J_loop();
00419 //while(1){
00420 //     
00421 //     myservo.position(40);
00422 //     wait(3);
00423 //      myservo.position(-40);
00424 //      wait(3);
00425 //       myservo.position(0);
00426 //        wait(3);
00427 //    
00428 //    }
00429 
00430 }
00431 
00432 
00433 
00434 /*******************************start AI of jihad*******************************/
00435 void wait_with_jihad_s(double i){
00436     jihad_time=true;
00437     int j=0;
00438     i*=1000;
00439     while(jihad_timer){
00440         wait_ms(1);
00441         j++;
00442         if(j>=i)jihad_timer=false;
00443         }
00444     }
00445 void jihad_tester_loop(){
00446     setSpeed(60);
00447     while (1) {
00448         int8_t i;
00449         getFeatures(0x01, 0x07);
00450         for (int i = 0; i < numberOfVector; i++) {
00451             Check_cwe3(vectors[i]);
00452             }
00453         if(!cou3_3al_chmel&&!cou3_3al_yamin)
00454             myservo.position(0);
00455         if(cou3_3al_chmel)
00456             cawi3_chmel();
00457         if(cou3_3al_yamin)
00458             cawi3_yamin();
00459         
00460         //wait_ms(50);
00461         cou3_3al_chmel=false;
00462         cou3_3al_yamin=false;   
00463         Part1=false;
00464         Part2=false;
00465         }
00466     }
00467 //myservo.position(0);
00468     
00469     
00470     
00471     
00472     
00473 
00474 void Check_cwe3(Vector v){
00475      if(v.m_y0<35&&v.m_y1>15&&v.m_x0<35&&v.m_x1>35)
00476         cou3_3al_chmel=true;
00477      if(v.m_y1<35&&v.m_y0>15&&v.m_x1<35&&v.m_x0>35)
00478         cou3_3al_chmel=true;
00479      if(v.m_y0<35&&v.m_y1>15&&v.m_x0>35&&v.m_x1<35)
00480         cou3_3al_yamin=true;
00481      if(v.m_y1<35&&v.m_y0>15&&v.m_x1>35&&v.m_x0<35)
00482         cou3_3al_yamin=true;
00483     
00484     
00485     }
00486 void check_the_end(){
00487         int three_nb=0;
00488         for(int i = 0; i < numberOfVector; i++) {
00489             if ( (vectors[i].m_x0<65&&vectors[i].m_x0>10)||(vectors[i].m_x1<65&&vectors[i].m_x1>10))
00490                 if ( (vectors[i].m_y0>10&&vectors[i].m_y0<20)||(vectors[i].m_y1>10&&vectors[i].m_y1<20))
00491                     three_nb++;
00492             }
00493         if(three_nb>=3){
00494             wait_ms(500);
00495             the_end=true;
00496             setSpeed(0);
00497             }
00498     }
00499 void rouh_chmel(){
00500     myservo.position(-45);
00501     
00502     wait_ms(50);
00503     //myservo.position(0);
00504     }
00505 
00506 void rouh_yamin(){
00507     myservo.position(45);
00508     wait_ms(50);
00509     //myservo.position(0);
00510     }
00511 void cawi3_yamin(){
00512     myservo.position(40);
00513     
00514 //   wait_ms(1000);
00515   //  myservo.position(0);
00516     }
00517      
00518 void cawi3_chmel(){
00519     myservo.position(-40);
00520 //    wait_ms(1000);
00521   // wait(1.5);
00522 //    myservo.position(0);
00523     }
00524 
00525 void Check_msalbiye(){
00526      if(numberOfIntersections>=4){
00527         Part1=true;
00528         Part2=true;
00529         wait(1);}
00530     }
00531 
00532 
00533 void Check_intersections(){
00534     bool cou3_yamin=false;
00535     bool cou3_chmel=false;
00536     bool mata3mel_chi=false;
00537         for(int i=0;i<numberOfIntersections;i++){
00538             if(intersections[i].m_y<15)
00539                if(intersections[i].m_x<20)
00540                     cou3_yamin=true;
00541              if(intersections[i].m_y<15)
00542                if(intersections[i].m_x>68)
00543                     cou3_chmel=true;
00544             }
00545     if(cou3_chmel&&cou3_yamin)
00546         mata3mel_chi=true;
00547     if(!mata3mel_chi){
00548         if(cou3_chmel)
00549             cawi3_chmel();
00550         if(cou3_yamin)
00551             cawi3_yamin();
00552         }
00553     }
00554 
00555 
00556 
00557 void Check_vector_left(Vector v){
00558     if(v.m_y0<16&&v.m_y1>45&&v.m_x0<35&&v.m_x1<35)
00559         Part1=true;
00560     if(v.m_y1<16&&v.m_y0>45&&v.m_x0<35&&v.m_x1<35)
00561         Part1=true;
00562     }
00563     
00564 void Check_vector_right(Vector v){
00565     printf("check_vect_right");
00566     if(v.m_y0<16&&v.m_y1>30&&v.m_x0>35&&v.m_x1>35)
00567         Part2=true;
00568     if(v.m_y1<16&&v.m_y0>30&&v.m_x0>35&&v.m_x1>35)
00569         Part2=true;
00570     }
00571 
00572 void check_lines(){
00573     
00574     for (int i = 0; i < numberOfVector; i++) {
00575             Print(i);
00576             Check_vector_left(vectors[i]);
00577             Check_vector_right(vectors[i]);
00578         }
00579     }
00580 
00581 void J_loop(){
00582     //motorLeft =0.8;
00583     //motorRight =0.8;
00584    // printf("entering_jihad loop");
00585     setSpeed(50);
00586     while(!the_end){
00587         int8_t i;
00588         getFeatures(0x01, 0x07);
00589         
00590         //if(numberOfVector >= 5)
00591            // check_the_end();
00592         
00593         
00594         if (numberOfVector > 0) {
00595             check_lines();
00596             }
00597        // Check_msalbiye();    
00598 //        if(!Part1)
00599 //            rouh_chmel();
00600 //        if(!Part2)
00601 //            rouh_yamin();
00602         
00603         if(Part2&&Part1)
00604             myservo.position(0);
00605         
00606           
00607         // if(numberOfIntersections>=2)
00608         //      Check_intersections(); 
00609             
00610         Part1=false;
00611         Part2=false;
00612         }
00613     //setSpeed(0);
00614     }
00615 
00616