jihad al akl / Mbed 2 deprecated atests_car

-include wait_with_jihad_s -a mini ai that need interrupt

Dependencies:   mbed Servo

main.cpp@1:202a0e209b73, 2020-03-03 (annotated)

Committer:
vjixy
Date:
Tue Mar 03 11:52:33 2020 +0000
Revision:
1:202a0e209b73
Parent:
0:b00ca4083336 
l;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vjixy 0:b00ca4083336 1 #include "mbed.h"
vjixy 0:b00ca4083336 2 #include "Servo.h"
vjixy 0:b00ca4083336 3 //CONNCTION OF PIXY WITH FRDM
vjixy 0:b00ca4083336 4 #define mosiPin PTD3
vjixy 0:b00ca4083336 5 #define misoPin PTD2
vjixy 0:b00ca4083336 6 #define clockPin PTD1
vjixy 0:b00ca4083336 7 #define sspin PTD0
vjixy 0:b00ca4083336 8
vjixy 0:b00ca4083336 9 #define speed_Limit 10
vjixy 0:b00ca4083336 10
vjixy 0:b00ca4083336 11 #define PIXY_SPI_CLOCKRATE 2000000
vjixy 0:b00ca4083336 12
vjixy 0:b00ca4083336 13
vjixy 0:b00ca4083336 14 //first bytes of pixy 2 packets with/without checksum packets;
vjixy 0:b00ca4083336 15 #define hex_no_Checksum_First_Byte 0xAE
vjixy 0:b00ca4083336 16 #define hex_no_Checksum_Second_Byte 0xC1
vjixy 0:b00ca4083336 17 #define hex_Checksum_First_Byte 0xAF
vjixy 0:b00ca4083336 18 //type
vjixy 0:b00ca4083336 19 #define hex_Set_Lamp_Request 0X16//set Lamp request
vjixy 0:b00ca4083336 20 #define hex_Get_Main_Features_Request 0X30//Get Main_Features request
vjixy 0:b00ca4083336 21 #define hex_Get_Main_Features_Response 0X31//Get Main_Features response
vjixy 0:b00ca4083336 22 #define hex_Set_Mode_Request 0X36//Set Mode request
vjixy 0:b00ca4083336 23 #define hex_Set_Next_Turn_Request 0X3A//Set Next Turn request
vjixy 0:b00ca4083336 24 #define hex_Set_Default_Turn_Request 0X3C//Set Default Turn request
vjixy 0:b00ca4083336 25 #define hex_Set_Vector_Request 0X38//Set Vector request
vjixy 0:b00ca4083336 26 #define hex_Revese_Vector_Request 0X3E//Reverse Vector request
vjixy 0:b00ca4083336 27 #define PIXY_TYPE_REQUEST_CHANGE_PROG 0x02//request program change
vjixy 0:b00ca4083336 28
vjixy 0:b00ca4083336 29 #define PIXY_MAX_PROGNAME 0x21//change programe
vjixy 0:b00ca4083336 30 #define LINE_MAX_INTERSECTION_LINES 30
vjixy 0:b00ca4083336 31 //the Feature is set with an or request -->3->Vectors+ Intersection
vjixy 0:b00ca4083336 32 #define FeatureVector 1
vjixy 0:b00ca4083336 33 #define FeatureIntersection 2
vjixy 0:b00ca4083336 34 #define FeatureBarcode 4
vjixy 0:b00ca4083336 35
vjixy 0:b00ca4083336 36 struct Vector {
vjixy 0:b00ca4083336 37
vjixy 0:b00ca4083336 38
vjixy 0:b00ca4083336 39 uint8_t m_x0;
vjixy 0:b00ca4083336 40 uint8_t m_y0;
vjixy 0:b00ca4083336 41 uint8_t m_x1;
vjixy 0:b00ca4083336 42 uint8_t m_y1;
vjixy 0:b00ca4083336 43 uint8_t m_index;
vjixy 0:b00ca4083336 44 uint8_t m_flags;
vjixy 0:b00ca4083336 45 };
vjixy 0:b00ca4083336 46
vjixy 0:b00ca4083336 47 struct IntersectionLine {
vjixy 0:b00ca4083336 48 uint8_t m_index;
vjixy 0:b00ca4083336 49 uint8_t m_reserved;
vjixy 0:b00ca4083336 50 int16_t m_angle;
vjixy 0:b00ca4083336 51 };
vjixy 0:b00ca4083336 52
vjixy 0:b00ca4083336 53 struct Intersection {
vjixy 0:b00ca4083336 54 void print()
vjixy 0:b00ca4083336 55 {
vjixy 0:b00ca4083336 56 char buf[64];
vjixy 0:b00ca4083336 57 uint8_t i;
vjixy 0:b00ca4083336 58 sprintf(buf, "intersection: (%d %d)", m_x, m_y);
vjixy 0:b00ca4083336 59 printf("%s\n", buf);
vjixy 0:b00ca4083336 60 for (i = 0; i < m_n; i++) {
vjixy 0:b00ca4083336 61 sprintf(buf, " %d: index: %d angle: %d", i, m_intLines[i].m_index, m_intLines[i].m_angle);
vjixy 0:b00ca4083336 62 printf("%s\n", buf);
vjixy 0:b00ca4083336 63 }
vjixy 0:b00ca4083336 64 }
vjixy 0:b00ca4083336 65
vjixy 0:b00ca4083336 66 uint8_t m_x;
vjixy 0:b00ca4083336 67 uint8_t m_y;
vjixy 0:b00ca4083336 68
vjixy 0:b00ca4083336 69 uint8_t m_n;
vjixy 0:b00ca4083336 70 uint8_t m_reserved;
vjixy 0:b00ca4083336 71 IntersectionLine m_intLines[LINE_MAX_INTERSECTION_LINES];
vjixy 0:b00ca4083336 72 };
vjixy 0:b00ca4083336 73
vjixy 0:b00ca4083336 74 struct Barcode {
vjixy 0:b00ca4083336 75 void print()
vjixy 0:b00ca4083336 76 {
vjixy 0:b00ca4083336 77 char buf[64];
vjixy 0:b00ca4083336 78 sprintf(buf, "Barcode: (%d %d), val: %d flags: %d", m_x, m_y, m_code, m_flags);
vjixy 0:b00ca4083336 79 printf("%s\n", buf);
vjixy 0:b00ca4083336 80 }
vjixy 0:b00ca4083336 81
vjixy 0:b00ca4083336 82 uint8_t m_x;
vjixy 0:b00ca4083336 83 uint8_t m_y;
vjixy 0:b00ca4083336 84 uint8_t m_flags;
vjixy 0:b00ca4083336 85 uint8_t m_code;
vjixy 0:b00ca4083336 86 };
vjixy 0:b00ca4083336 87
vjixy 0:b00ca4083336 88 uint8_t Buffer[40];
vjixy 0:b00ca4083336 89 Vector vectors[20];
vjixy 0:b00ca4083336 90 Intersection intersections[10];
vjixy 0:b00ca4083336 91 Barcode barcodes[10];
vjixy 0:b00ca4083336 92 int length = 0;
vjixy 0:b00ca4083336 93 int numberOfVector = 0;
vjixy 0:b00ca4083336 94 int numberOfIntersections = 0;
vjixy 0:b00ca4083336 95 int numberOfbarcodes = 0;
vjixy 0:b00ca4083336 96
vjixy 0:b00ca4083336 97
vjixy 0:b00ca4083336 98 double angle = 0;
vjixy 0:b00ca4083336 99 int biggest_nb;
vjixy 0:b00ca4083336 100 int biggest_y;
vjixy 0:b00ca4083336 101 int speed;
vjixy 0:b00ca4083336 102
vjixy 0:b00ca4083336 103
vjixy 0:b00ca4083336 104 SPI spi(mosiPin, misoPin, clockPin);//mosi,miso,clock
vjixy 0:b00ca4083336 105 DigitalOut ssPin(sspin, 1);//pin for slave selection
vjixy 0:b00ca4083336 106
vjixy 0:b00ca4083336 107 Servo motorLeft(PTE20);
vjixy 0:b00ca4083336 108 Servo motorRight(PTE21);
vjixy 0:b00ca4083336 109 Servo myservo(PTE29);
vjixy 0:b00ca4083336 110
vjixy 0:b00ca4083336 111
vjixy 0:b00ca4083336 112
vjixy 0:b00ca4083336 113 bool cheksumValidation(uint8_t *Buff, uint8_t len)
vjixy 0:b00ca4083336 114 {
vjixy 0:b00ca4083336 115 if (len < 6) {
vjixy 0:b00ca4083336 116 printf("Len <6");
vjixy 0:b00ca4083336 117 return false;
vjixy 0:b00ca4083336 118 }
vjixy 0:b00ca4083336 119 uint16_t Checksum = Buff[5] << 8 | Buff[4];
vjixy 0:b00ca4083336 120 uint16_t sum = 0x0000;
vjixy 0:b00ca4083336 121 for (int i = 6; i < len; i++)
vjixy 0:b00ca4083336 122 sum += Buff[i];
vjixy 0:b00ca4083336 123 return sum == Checksum;
vjixy 0:b00ca4083336 124
vjixy 0:b00ca4083336 125 }
vjixy 0:b00ca4083336 126
vjixy 0:b00ca4083336 127 void clearBuffer(uint8_t *buff)
vjixy 0:b00ca4083336 128 {
vjixy 0:b00ca4083336 129 for (int i = 0; i < 40; i++)
vjixy 0:b00ca4083336 130 buff[i] = 0x00;
vjixy 0:b00ca4083336 131 }
vjixy 0:b00ca4083336 132
vjixy 0:b00ca4083336 133 int16_t recv(uint8_t *buf)
vjixy 0:b00ca4083336 134 {
vjixy 0:b00ca4083336 135 uint16_t *cs = NULL;
vjixy 0:b00ca4083336 136 if (cs)
vjixy 0:b00ca4083336 137 *cs = 0;
vjixy 0:b00ca4083336 138 uint8_t len;
vjixy 0:b00ca4083336 139 uint8_t i;
vjixy 0:b00ca4083336 140 ssPin = 0;
vjixy 0:b00ca4083336 141 bool frst = false;
vjixy 0:b00ca4083336 142 for (i = 0; i < 4; i++) {
vjixy 0:b00ca4083336 143 if (i == 0)
vjixy 0:b00ca4083336 144 do {
vjixy 0:b00ca4083336 145 buf[i] = spi.write(0x00);
vjixy 0:b00ca4083336 146
vjixy 0:b00ca4083336 147 if (cs)
vjixy 0:b00ca4083336 148 *cs += buf[i];
vjixy 0:b00ca4083336 149 if (buf[i] == 0xaf)
vjixy 0:b00ca4083336 150 frst = true;
vjixy 0:b00ca4083336 151
vjixy 0:b00ca4083336 152
vjixy 0:b00ca4083336 153 } while (!frst);
vjixy 0:b00ca4083336 154 else {
vjixy 0:b00ca4083336 155 buf[i] = spi.write(0x00);
vjixy 0:b00ca4083336 156
vjixy 0:b00ca4083336 157 if (cs)
vjixy 0:b00ca4083336 158 *cs += buf[i];
vjixy 0:b00ca4083336 159
vjixy 0:b00ca4083336 160 }
vjixy 0:b00ca4083336 161 }
vjixy 0:b00ca4083336 162 if (buf[0] == hex_Checksum_First_Byte)
vjixy 0:b00ca4083336 163 len = 2 + buf[3];
vjixy 0:b00ca4083336 164 else len = buf[3];
vjixy 0:b00ca4083336 165
vjixy 0:b00ca4083336 166 for (i = 0; i < 35; i++) {
vjixy 0:b00ca4083336 167 buf[i + 4] = spi.write(0x00);
vjixy 0:b00ca4083336 168 }
vjixy 0:b00ca4083336 169 len += 4;
vjixy 0:b00ca4083336 170 ssPin = 1;
vjixy 0:b00ca4083336 171 return len;
vjixy 0:b00ca4083336 172 }
vjixy 0:b00ca4083336 173
vjixy 0:b00ca4083336 174 int16_t send(uint8_t *buf, uint8_t len)
vjixy 0:b00ca4083336 175 {
vjixy 0:b00ca4083336 176 uint8_t i;
vjixy 0:b00ca4083336 177 ssPin = 0;
vjixy 0:b00ca4083336 178 for (i = 0; i < len; i++)
vjixy 0:b00ca4083336 179 spi.write(buf[i]);
vjixy 0:b00ca4083336 180 ssPin = 1;
vjixy 0:b00ca4083336 181 return len;
vjixy 0:b00ca4083336 182 }
vjixy 0:b00ca4083336 183
vjixy 0:b00ca4083336 184 void getMainFeaturesHandler(uint8_t *buf, uint8_t len)
vjixy 0:b00ca4083336 185 {
vjixy 0:b00ca4083336 186 numberOfVector = 0;
vjixy 0:b00ca4083336 187 numberOfbarcodes = 0;
vjixy 0:b00ca4083336 188 numberOfIntersections = 0;
vjixy 0:b00ca4083336 189 printf("CHecking Features\n");
vjixy 0:b00ca4083336 190 for (int i = 6; i < len; i++) {
vjixy 0:b00ca4083336 191 switch (buf[i]) {
vjixy 0:b00ca4083336 192 case FeatureVector:
vjixy 0:b00ca4083336 193 ++i;
vjixy 0:b00ca4083336 194 printf("found vector\n");
vjixy 0:b00ca4083336 195 vectors[numberOfVector].m_x0 = buf[++i];
vjixy 0:b00ca4083336 196 vectors[numberOfVector].m_y0 = buf[++i];
vjixy 0:b00ca4083336 197 vectors[numberOfVector].m_x1 = buf[++i];
vjixy 0:b00ca4083336 198 vectors[numberOfVector].m_y1 = buf[++i];
vjixy 0:b00ca4083336 199 vectors[numberOfVector].m_index = buf[++i];
vjixy 0:b00ca4083336 200 vectors[numberOfVector].m_flags = buf[++i];
vjixy 0:b00ca4083336 201 numberOfVector++;
vjixy 0:b00ca4083336 202 break;
vjixy 0:b00ca4083336 203 case FeatureIntersection:
vjixy 0:b00ca4083336 204 ++i;
vjixy 0:b00ca4083336 205 intersections[numberOfIntersections].m_x = buf[++i];
vjixy 0:b00ca4083336 206 intersections[numberOfIntersections].m_y = buf[++i];
vjixy 0:b00ca4083336 207 intersections[numberOfIntersections].m_n = buf[++i];
vjixy 0:b00ca4083336 208 intersections[numberOfIntersections].m_reserved = buf[++i];
vjixy 0:b00ca4083336 209 for (int j = 0; j < LINE_MAX_INTERSECTION_LINES; j++) {
vjixy 0:b00ca4083336 210 intersections[numberOfIntersections].m_intLines->m_index = buf[++i];
vjixy 0:b00ca4083336 211 intersections[numberOfIntersections].m_intLines->m_reserved = buf[++i];
vjixy 0:b00ca4083336 212 intersections[numberOfIntersections].m_intLines->m_angle = buf[++i];
vjixy 0:b00ca4083336 213 }
vjixy 0:b00ca4083336 214 break;
vjixy 0:b00ca4083336 215 case FeatureBarcode:
vjixy 0:b00ca4083336 216 i++;
vjixy 0:b00ca4083336 217 barcodes[numberOfbarcodes].m_x = buf[++i];
vjixy 0:b00ca4083336 218 barcodes[numberOfbarcodes].m_y = buf[++i];
vjixy 0:b00ca4083336 219 barcodes[numberOfbarcodes].m_flags = buf[++i];
vjixy 0:b00ca4083336 220 barcodes[numberOfbarcodes].m_code = buf[++i];
vjixy 0:b00ca4083336 221 break;
vjixy 0:b00ca4083336 222 }
vjixy 0:b00ca4083336 223 }
vjixy 0:b00ca4083336 224 }
vjixy 0:b00ca4083336 225
vjixy 0:b00ca4083336 226 void setLamp(uint8_t value1, uint8_t value2)
vjixy 0:b00ca4083336 227 {
vjixy 0:b00ca4083336 228 clearBuffer(Buffer);
vjixy 0:b00ca4083336 229 Buffer[0] = hex_no_Checksum_First_Byte;
vjixy 0:b00ca4083336 230 Buffer[1] = hex_no_Checksum_Second_Byte;
vjixy 0:b00ca4083336 231 Buffer[2] = hex_Set_Lamp_Request;
vjixy 0:b00ca4083336 232 Buffer[3] = 0x02;
vjixy 0:b00ca4083336 233 Buffer[4] = value1;
vjixy 0:b00ca4083336 234 Buffer[5] = value2;
vjixy 0:b00ca4083336 235 printf("V1:%d,V2:%d\n\r", value1, value2);
vjixy 0:b00ca4083336 236 int16_t len = send(Buffer, 6);
vjixy 0:b00ca4083336 237 len = recv(Buffer);
vjixy 0:b00ca4083336 238
vjixy 0:b00ca4083336 239 }
vjixy 0:b00ca4083336 240
vjixy 0:b00ca4083336 241 void getFeatures(uint8_t type, uint8_t Features)
vjixy 0:b00ca4083336 242 {
vjixy 0:b00ca4083336 243
vjixy 0:b00ca4083336 244 clearBuffer(Buffer);
vjixy 0:b00ca4083336 245 Buffer[0] = hex_no_Checksum_First_Byte;
vjixy 0:b00ca4083336 246 Buffer[1] = hex_no_Checksum_Second_Byte;
vjixy 0:b00ca4083336 247 Buffer[2] = hex_Get_Main_Features_Request;
vjixy 0:b00ca4083336 248 Buffer[3] = 0x02;
vjixy 0:b00ca4083336 249 Buffer[4] = type;
vjixy 0:b00ca4083336 250 Buffer[5] = Features;
vjixy 0:b00ca4083336 251
vjixy 0:b00ca4083336 252 int16_t len = send(Buffer, 6);
vjixy 0:b00ca4083336 253 printf("Getting features\n");
vjixy 0:b00ca4083336 254
vjixy 0:b00ca4083336 255 len = recv(Buffer);
vjixy 0:b00ca4083336 256 getMainFeaturesHandler(Buffer, len);
vjixy 0:b00ca4083336 257 }
vjixy 0:b00ca4083336 258
vjixy 0:b00ca4083336 259 void changeProg(const char *prog)
vjixy 0:b00ca4083336 260 {
vjixy 0:b00ca4083336 261 clearBuffer(Buffer);
vjixy 0:b00ca4083336 262 Buffer[0] = hex_no_Checksum_First_Byte;
vjixy 0:b00ca4083336 263 Buffer[1] = hex_no_Checksum_Second_Byte;
vjixy 0:b00ca4083336 264 Buffer[2] = PIXY_TYPE_REQUEST_CHANGE_PROG;
vjixy 0:b00ca4083336 265 Buffer[3] = strlen(prog);
vjixy 0:b00ca4083336 266 length = send(Buffer, Buffer[3] + 4);
vjixy 0:b00ca4083336 267 length = recv(Buffer);
vjixy 0:b00ca4083336 268
vjixy 0:b00ca4083336 269 }
vjixy 0:b00ca4083336 270
vjixy 0:b00ca4083336 271
vjixy 0:b00ca4083336 272
vjixy 0:b00ca4083336 273 void Print(int i)
vjixy 0:b00ca4083336 274 {
vjixy 0:b00ca4083336 275 char buf[64];
vjixy 0:b00ca4083336 276 sprintf(buf, "vector: (%d %d) (%d %d) index: %d flags %d", vectors[i].m_x0, vectors[i].m_y0, vectors[i].m_x1,
vjixy 0:b00ca4083336 277 vectors[i].m_y1, vectors[i].m_index, vectors[i].m_flags);
vjixy 0:b00ca4083336 278 printf("%s\n", buf);
vjixy 0:b00ca4083336 279 }
vjixy 0:b00ca4083336 280
vjixy 0:b00ca4083336 281 void setSpeed(int speed)
vjixy 0:b00ca4083336 282 {
vjixy 0:b00ca4083336 283 printf("CHanged Speed\n");
vjixy 0:b00ca4083336 284 double var=speed*speed_Limit/100;
vjixy 0:b00ca4083336 285 var = var/100 +0.5;
vjixy 0:b00ca4083336 286 motorLeft = var;
vjixy 0:b00ca4083336 287 motorRight = var;
vjixy 0:b00ca4083336 288 }
vjixy 0:b00ca4083336 289
vjixy 0:b00ca4083336 290 void arm()//arm the esc
vjixy 0:b00ca4083336 291 {
vjixy 0:b00ca4083336 292 motorLeft = 0.0;
vjixy 0:b00ca4083336 293 motorRight = 0.0;
vjixy 0:b00ca4083336 294 wait(0.5); //detects signal
vjixy 0:b00ca4083336 295 //Required ESC Calibration/Arming sequence
vjixy 0:b00ca4083336 296 //sends longest and shortest PWM pulse to learn and arm at power on
vjixy 0:b00ca4083336 297 motorLeft = 1.0; //send longest PWM
vjixy 0:b00ca4083336 298 motorRight = 1.0;
vjixy 0:b00ca4083336 299 wait(2);
vjixy 0:b00ca4083336 300 motorLeft = 0.0; //send shortest PWM
vjixy 0:b00ca4083336 301 motorRight = 0.0;
vjixy 0:b00ca4083336 302 wait(2);
vjixy 0:b00ca4083336 303 //ESC now operational using standard servo PWM signals
vjixy 0:b00ca4083336 304 for (float p = 0; p <= 0.5; p += 0.025) {
vjixy 0:b00ca4083336 305 printf("Arming underway\n");
vjixy 0:b00ca4083336 306 motorLeft = p;
vjixy 0:b00ca4083336 307 motorRight = p;
vjixy 0:b00ca4083336 308 wait_ms(100);
vjixy 0:b00ca4083336 309
vjixy 0:b00ca4083336 310
vjixy 0:b00ca4083336 311
vjixy 0:b00ca4083336 312 }
vjixy 0:b00ca4083336 313 printf("leaving arming");
vjixy 0:b00ca4083336 314
vjixy 0:b00ca4083336 315 }
vjixy 0:b00ca4083336 316
vjixy 0:b00ca4083336 317 void loop()
vjixy 0:b00ca4083336 318 {
vjixy 0:b00ca4083336 319
vjixy 0:b00ca4083336 320 //Serial.println(angle);
vjixy 0:b00ca4083336 321
vjixy 0:b00ca4083336 322 int8_t i;
vjixy 0:b00ca4083336 323 getFeatures(0x01, 0x07);
vjixy 0:b00ca4083336 324
vjixy 0:b00ca4083336 325 if (numberOfVector > 0) {
vjixy 0:b00ca4083336 326 biggest_nb = 0;
vjixy 0:b00ca4083336 327 biggest_y = vectors[0].m_y1 - vectors[0].m_y0;
vjixy 0:b00ca4083336 328 for (i = 0; i < numberOfVector; i++) {
vjixy 0:b00ca4083336 329 if ((vectors[i].m_y1 - vectors[i].m_y0) > biggest_y) {
vjixy 0:b00ca4083336 330 biggest_y = vectors[i].m_y1 - vectors[i].m_y0;
vjixy 0:b00ca4083336 331 biggest_nb = i;
vjixy 0:b00ca4083336 332 }
vjixy 0:b00ca4083336 333 }
vjixy 0:b00ca4083336 334
vjixy 0:b00ca4083336 335
vjixy 0:b00ca4083336 336 // for (i = 0; i < numberOfVector; i++)
vjixy 0:b00ca4083336 337 // {
vjixy 0:b00ca4083336 338 Print(biggest_nb);
vjixy 0:b00ca4083336 339 int x0 = vectors[biggest_nb].m_x0;
vjixy 0:b00ca4083336 340 int x1 = vectors[biggest_nb].m_x1;
vjixy 0:b00ca4083336 341 int x = x0 - x1;
vjixy 0:b00ca4083336 342 int y0 = vectors[biggest_nb].m_y0;
vjixy 0:b00ca4083336 343 int y1 = vectors[biggest_nb].m_y1;
vjixy 0:b00ca4083336 344 int y = y0 - y1;
vjixy 0:b00ca4083336 345 if (y < 0)y *= -1;
vjixy 0:b00ca4083336 346 double param= y / x;
vjixy 0:b00ca4083336 347 angle = atan(param) * 180/3.14;
vjixy 0:b00ca4083336 348 printf("angle:%d\n", angle);
vjixy 0:b00ca4083336 349
vjixy 0:b00ca4083336 350
vjixy 0:b00ca4083336 351 if (angle > -40 || angle < 40) {
vjixy 0:b00ca4083336 352 myservo.position(angle);
vjixy 0:b00ca4083336 353 setSpeed(30);
vjixy 0:b00ca4083336 354 } else {
vjixy 0:b00ca4083336 355 myservo.position(0);
vjixy 0:b00ca4083336 356 setSpeed(50);
vjixy 0:b00ca4083336 357 }
vjixy 0:b00ca4083336 358 wait_ms(100);
vjixy 0:b00ca4083336 359 } else {
vjixy 0:b00ca4083336 360 myservo.position(angle);
vjixy 0:b00ca4083336 361 setSpeed(0);
vjixy 0:b00ca4083336 362 wait_ms(50);
vjixy 0:b00ca4083336 363 }
vjixy 0:b00ca4083336 364
vjixy 0:b00ca4083336 365 }
vjixy 0:b00ca4083336 366
vjixy 0:b00ca4083336 367
vjixy 0:b00ca4083336 368
vjixy 0:b00ca4083336 369
vjixy 0:b00ca4083336 370 /*******************************start part for jihad*******************************/
vjixy 0:b00ca4083336 371 bool Part1=false;bool cou3_3al_yamin=false;
vjixy 0:b00ca4083336 372 bool Part2=false;bool cou3_3al_chmel=false;
vjixy 0:b00ca4083336 373 bool msalbiye_detected=false;
vjixy 0:b00ca4083336 374 bool the_end=false;
vjixy 0:b00ca4083336 375 void check_the_end();
vjixy 0:b00ca4083336 376 void rouh_chmel();
vjixy 0:b00ca4083336 377 void rouh_yamin();
vjixy 0:b00ca4083336 378 void cawi3_yamin();
vjixy 0:b00ca4083336 379 void cawi3_chmel();
vjixy 0:b00ca4083336 380 void Check_msalbiye();
vjixy 0:b00ca4083336 381 void Check_vector_left(Vector v);
vjixy 0:b00ca4083336 382 void Check_vector_right(Vector v);
vjixy 0:b00ca4083336 383 void check_lines();
vjixy 0:b00ca4083336 384 void J_loop();
vjixy 0:b00ca4083336 385 void Check_intersections();
vjixy 0:b00ca4083336 386 void jihad_tester_loop();
vjixy 0:b00ca4083336 387 void Check_cwe3(Vector v);
vjixy 0:b00ca4083336 388 void wait_with_jihad(double i);
vjixy 0:b00ca4083336 389 bool jihad_timer_s=false;
vjixy 1:202a0e209b73 390 Ticker tick;
vjixy 1:202a0e209b73 391 Timer t;
vjixy 1:202a0e209b73 392
vjixy 0:b00ca4083336 393 /**********************************end part for jihad******************************/
vjixy 0:b00ca4083336 394
vjixy 0:b00ca4083336 395
vjixy 0:b00ca4083336 396
vjixy 0:b00ca4083336 397
vjixy 0:b00ca4083336 398 int main()
vjixy 0:b00ca4083336 399 {
vjixy 0:b00ca4083336 400 spi.format(8, 0);
vjixy 0:b00ca4083336 401 spi.frequency(PIXY_SPI_CLOCKRATE);
vjixy 0:b00ca4083336 402 myservo.calibrate(0.001, 90);
vjixy 0:b00ca4083336 403 myservo.write(0.5);
vjixy 0:b00ca4083336 404 //call the esc arming function
vjixy 0:b00ca4083336 405 arm();
vjixy 0:b00ca4083336 406 // while(1)
vjixy 0:b00ca4083336 407 //{ motorLeft =0.8;
vjixy 0:b00ca4083336 408 // motorRight =0.8;}
vjixy 0:b00ca4083336 409 //printf("process started");
vjixy 0:b00ca4083336 410 // setSpeed(100);
vjixy 0:b00ca4083336 411 //printf("process started");
vjixy 0:b00ca4083336 412 changeProg("line");
vjixy 0:b00ca4083336 413 printf("process started");
vjixy 0:b00ca4083336 414 //setSpeed(50);
vjixy 0:b00ca4083336 415 jihad_tester_loop();
vjixy 0:b00ca4083336 416
vjixy 0:b00ca4083336 417 //printf("process started");
vjixy 0:b00ca4083336 418 // J_loop();
vjixy 0:b00ca4083336 419 //while(1){
vjixy 0:b00ca4083336 420 //
vjixy 0:b00ca4083336 421 // myservo.position(40);
vjixy 0:b00ca4083336 422 // wait(3);
vjixy 0:b00ca4083336 423 // myservo.position(-40);
vjixy 0:b00ca4083336 424 // wait(3);
vjixy 0:b00ca4083336 425 // myservo.position(0);
vjixy 0:b00ca4083336 426 // wait(3);
vjixy 0:b00ca4083336 427 //
vjixy 0:b00ca4083336 428 // }
vjixy 0:b00ca4083336 429
vjixy 0:b00ca4083336 430 }
vjixy 0:b00ca4083336 431
vjixy 0:b00ca4083336 432
vjixy 0:b00ca4083336 433
vjixy 0:b00ca4083336 434 /*******************************start AI of jihad*******************************/
vjixy 0:b00ca4083336 435 void wait_with_jihad_s(double i){
vjixy 0:b00ca4083336 436 jihad_time=true;
vjixy 0:b00ca4083336 437 int j=0;
vjixy 0:b00ca4083336 438 i*=1000;
vjixy 0:b00ca4083336 439 while(jihad_timer){
vjixy 0:b00ca4083336 440 wait_ms(1);
vjixy 0:b00ca4083336 441 j++;
vjixy 0:b00ca4083336 442 if(j>=i)jihad_timer=false;
vjixy 0:b00ca4083336 443 }
vjixy 0:b00ca4083336 444 }
vjixy 0:b00ca4083336 445 void jihad_tester_loop(){
vjixy 0:b00ca4083336 446 setSpeed(60);
vjixy 0:b00ca4083336 447 while (1) {
vjixy 0:b00ca4083336 448 int8_t i;
vjixy 0:b00ca4083336 449 getFeatures(0x01, 0x07);
vjixy 0:b00ca4083336 450 for (int i = 0; i < numberOfVector; i++) {
vjixy 0:b00ca4083336 451 Check_cwe3(vectors[i]);
vjixy 0:b00ca4083336 452 }
vjixy 0:b00ca4083336 453 if(!cou3_3al_chmel&&!cou3_3al_yamin)
vjixy 0:b00ca4083336 454 myservo.position(0);
vjixy 0:b00ca4083336 455 if(cou3_3al_chmel)
vjixy 0:b00ca4083336 456 cawi3_chmel();
vjixy 0:b00ca4083336 457 if(cou3_3al_yamin)
vjixy 0:b00ca4083336 458 cawi3_yamin();
vjixy 0:b00ca4083336 459
vjixy 0:b00ca4083336 460 //wait_ms(50);
vjixy 0:b00ca4083336 461 cou3_3al_chmel=false;
vjixy 0:b00ca4083336 462 cou3_3al_yamin=false;
vjixy 0:b00ca4083336 463 Part1=false;
vjixy 0:b00ca4083336 464 Part2=false;
vjixy 0:b00ca4083336 465 }
vjixy 0:b00ca4083336 466 }
vjixy 0:b00ca4083336 467 //myservo.position(0);
vjixy 0:b00ca4083336 468
vjixy 0:b00ca4083336 469
vjixy 0:b00ca4083336 470
vjixy 0:b00ca4083336 471
vjixy 0:b00ca4083336 472
vjixy 0:b00ca4083336 473
vjixy 0:b00ca4083336 474 void Check_cwe3(Vector v){
vjixy 0:b00ca4083336 475 if(v.m_y0<35&&v.m_y1>15&&v.m_x0<35&&v.m_x1>35)
vjixy 0:b00ca4083336 476 cou3_3al_chmel=true;
vjixy 0:b00ca4083336 477 if(v.m_y1<35&&v.m_y0>15&&v.m_x1<35&&v.m_x0>35)
vjixy 0:b00ca4083336 478 cou3_3al_chmel=true;
vjixy 0:b00ca4083336 479 if(v.m_y0<35&&v.m_y1>15&&v.m_x0>35&&v.m_x1<35)
vjixy 0:b00ca4083336 480 cou3_3al_yamin=true;
vjixy 0:b00ca4083336 481 if(v.m_y1<35&&v.m_y0>15&&v.m_x1>35&&v.m_x0<35)
vjixy 0:b00ca4083336 482 cou3_3al_yamin=true;
vjixy 0:b00ca4083336 483
vjixy 0:b00ca4083336 484
vjixy 0:b00ca4083336 485 }
vjixy 0:b00ca4083336 486 void check_the_end(){
vjixy 0:b00ca4083336 487 int three_nb=0;
vjixy 0:b00ca4083336 488 for(int i = 0; i < numberOfVector; i++) {
vjixy 0:b00ca4083336 489 if ( (vectors[i].m_x0<65&&vectors[i].m_x0>10)||(vectors[i].m_x1<65&&vectors[i].m_x1>10))
vjixy 0:b00ca4083336 490 if ( (vectors[i].m_y0>10&&vectors[i].m_y0<20)||(vectors[i].m_y1>10&&vectors[i].m_y1<20))
vjixy 0:b00ca4083336 491 three_nb++;
vjixy 0:b00ca4083336 492 }
vjixy 0:b00ca4083336 493 if(three_nb>=3){
vjixy 0:b00ca4083336 494 wait_ms(500);
vjixy 0:b00ca4083336 495 the_end=true;
vjixy 0:b00ca4083336 496 setSpeed(0);
vjixy 0:b00ca4083336 497 }
vjixy 0:b00ca4083336 498 }
vjixy 0:b00ca4083336 499 void rouh_chmel(){
vjixy 0:b00ca4083336 500 myservo.position(-45);
vjixy 0:b00ca4083336 501
vjixy 0:b00ca4083336 502 wait_ms(50);
vjixy 0:b00ca4083336 503 //myservo.position(0);
vjixy 0:b00ca4083336 504 }
vjixy 0:b00ca4083336 505
vjixy 0:b00ca4083336 506 void rouh_yamin(){
vjixy 0:b00ca4083336 507 myservo.position(45);
vjixy 0:b00ca4083336 508 wait_ms(50);
vjixy 0:b00ca4083336 509 //myservo.position(0);
vjixy 0:b00ca4083336 510 }
vjixy 0:b00ca4083336 511 void cawi3_yamin(){
vjixy 0:b00ca4083336 512 myservo.position(40);
vjixy 0:b00ca4083336 513
vjixy 0:b00ca4083336 514 // wait_ms(1000);
vjixy 0:b00ca4083336 515 // myservo.position(0);
vjixy 0:b00ca4083336 516 }
vjixy 0:b00ca4083336 517
vjixy 0:b00ca4083336 518 void cawi3_chmel(){
vjixy 0:b00ca4083336 519 myservo.position(-40);
vjixy 0:b00ca4083336 520 // wait_ms(1000);
vjixy 0:b00ca4083336 521 // wait(1.5);
vjixy 0:b00ca4083336 522 // myservo.position(0);
vjixy 0:b00ca4083336 523 }
vjixy 0:b00ca4083336 524
vjixy 0:b00ca4083336 525 void Check_msalbiye(){
vjixy 0:b00ca4083336 526 if(numberOfIntersections>=4){
vjixy 0:b00ca4083336 527 Part1=true;
vjixy 0:b00ca4083336 528 Part2=true;
vjixy 0:b00ca4083336 529 wait(1);}
vjixy 0:b00ca4083336 530 }
vjixy 0:b00ca4083336 531
vjixy 0:b00ca4083336 532
vjixy 0:b00ca4083336 533 void Check_intersections(){
vjixy 0:b00ca4083336 534 bool cou3_yamin=false;
vjixy 0:b00ca4083336 535 bool cou3_chmel=false;
vjixy 0:b00ca4083336 536 bool mata3mel_chi=false;
vjixy 0:b00ca4083336 537 for(int i=0;i<numberOfIntersections;i++){
vjixy 0:b00ca4083336 538 if(intersections[i].m_y<15)
vjixy 0:b00ca4083336 539 if(intersections[i].m_x<20)
vjixy 0:b00ca4083336 540 cou3_yamin=true;
vjixy 0:b00ca4083336 541 if(intersections[i].m_y<15)
vjixy 0:b00ca4083336 542 if(intersections[i].m_x>68)
vjixy 0:b00ca4083336 543 cou3_chmel=true;
vjixy 0:b00ca4083336 544 }
vjixy 0:b00ca4083336 545 if(cou3_chmel&&cou3_yamin)
vjixy 0:b00ca4083336 546 mata3mel_chi=true;
vjixy 0:b00ca4083336 547 if(!mata3mel_chi){
vjixy 0:b00ca4083336 548 if(cou3_chmel)
vjixy 0:b00ca4083336 549 cawi3_chmel();
vjixy 0:b00ca4083336 550 if(cou3_yamin)
vjixy 0:b00ca4083336 551 cawi3_yamin();
vjixy 0:b00ca4083336 552 }
vjixy 0:b00ca4083336 553 }
vjixy 0:b00ca4083336 554
vjixy 0:b00ca4083336 555
vjixy 0:b00ca4083336 556
vjixy 0:b00ca4083336 557 void Check_vector_left(Vector v){
vjixy 0:b00ca4083336 558 if(v.m_y0<16&&v.m_y1>45&&v.m_x0<35&&v.m_x1<35)
vjixy 0:b00ca4083336 559 Part1=true;
vjixy 0:b00ca4083336 560 if(v.m_y1<16&&v.m_y0>45&&v.m_x0<35&&v.m_x1<35)
vjixy 0:b00ca4083336 561 Part1=true;
vjixy 0:b00ca4083336 562 }
vjixy 0:b00ca4083336 563
vjixy 0:b00ca4083336 564 void Check_vector_right(Vector v){
vjixy 0:b00ca4083336 565 printf("check_vect_right");
vjixy 0:b00ca4083336 566 if(v.m_y0<16&&v.m_y1>30&&v.m_x0>35&&v.m_x1>35)
vjixy 0:b00ca4083336 567 Part2=true;
vjixy 0:b00ca4083336 568 if(v.m_y1<16&&v.m_y0>30&&v.m_x0>35&&v.m_x1>35)
vjixy 0:b00ca4083336 569 Part2=true;
vjixy 0:b00ca4083336 570 }
vjixy 0:b00ca4083336 571
vjixy 0:b00ca4083336 572 void check_lines(){
vjixy 0:b00ca4083336 573
vjixy 0:b00ca4083336 574 for (int i = 0; i < numberOfVector; i++) {
vjixy 0:b00ca4083336 575 Print(i);
vjixy 0:b00ca4083336 576 Check_vector_left(vectors[i]);
vjixy 0:b00ca4083336 577 Check_vector_right(vectors[i]);
vjixy 0:b00ca4083336 578 }
vjixy 0:b00ca4083336 579 }
vjixy 0:b00ca4083336 580
vjixy 0:b00ca4083336 581 void J_loop(){
vjixy 0:b00ca4083336 582 //motorLeft =0.8;
vjixy 0:b00ca4083336 583 //motorRight =0.8;
vjixy 0:b00ca4083336 584 // printf("entering_jihad loop");
vjixy 0:b00ca4083336 585 setSpeed(50);
vjixy 0:b00ca4083336 586 while(!the_end){
vjixy 0:b00ca4083336 587 int8_t i;
vjixy 0:b00ca4083336 588 getFeatures(0x01, 0x07);
vjixy 0:b00ca4083336 589
vjixy 0:b00ca4083336 590 //if(numberOfVector >= 5)
vjixy 0:b00ca4083336 591 // check_the_end();
vjixy 0:b00ca4083336 592
vjixy 0:b00ca4083336 593
vjixy 0:b00ca4083336 594 if (numberOfVector > 0) {
vjixy 0:b00ca4083336 595 check_lines();
vjixy 0:b00ca4083336 596 }
vjixy 0:b00ca4083336 597 // Check_msalbiye();
vjixy 0:b00ca4083336 598 // if(!Part1)
vjixy 0:b00ca4083336 599 // rouh_chmel();
vjixy 0:b00ca4083336 600 // if(!Part2)
vjixy 0:b00ca4083336 601 // rouh_yamin();
vjixy 0:b00ca4083336 602
vjixy 0:b00ca4083336 603 if(Part2&&Part1)
vjixy 0:b00ca4083336 604 myservo.position(0);
vjixy 0:b00ca4083336 605
vjixy 0:b00ca4083336 606
vjixy 0:b00ca4083336 607 // if(numberOfIntersections>=2)
vjixy 0:b00ca4083336 608 // Check_intersections();
vjixy 0:b00ca4083336 609
vjixy 0:b00ca4083336 610 Part1=false;
vjixy 0:b00ca4083336 611 Part2=false;
vjixy 0:b00ca4083336 612 }
vjixy 0:b00ca4083336 613 //setSpeed(0);
vjixy 0:b00ca4083336 614 }
vjixy 0:b00ca4083336 615
vjixy 0:b00ca4083336 616