Diff: main.cpp
- Revision:
- 0:b00ca4083336
- Child:
- 1:202a0e209b73
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Mar 03 11:10:15 2020 +0000
@@ -0,0 +1,613 @@
+#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;
+/**********************************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);
+ }
+
+