diff --git a/Code/README.md b/Code/README.md new file mode 100644 index 0000000000000000000000000000000000000000..341efc49f06136575d31f499fa3a5c150401c5bd --- /dev/null +++ b/Code/README.md @@ -0,0 +1,325 @@ +# Review of Key Aspects of the System (10/01/2021) + +Meeting with Zhaoliang (10/01/2021), Recorded by Zhiying (10/04/2021) + +<img src="README_Graphs/0_cover.png" alt="0_cover" style="width: 75%;" /> + +## Current Hardware Setups + +<img src="README_Graphs/1_current_setup.png" alt="1_current_setup" style="width: 75%;" /> + +* There are 2 main module of the system + * **Camera**: Detect the target (green bolb). + * **Hardware**: OpenMV Cam H7 + * **Code**: [`openMV_main.py`](https://git.uclalemur.com/shahrulkamil98/blimp-autonomy/-/blob/main/Codes/OpenMV H7 Plus Camera/openMV_main.py) + * **Control Board**: Based on Inputs from Camera (Detect / NOT Detect), control motors (Catch / Seeking) + * **Hardware**: Feather ESP 32 + * **Code**: [`feather_main_personaldemo_PID.ino`](https://git.uclalemur.com/shahrulkamil98/november-2021-blimp-competition/-/blob/main/Code/Main Feather Code/feather_main_personaldemo_PID.ino) +* OpenMV camera (P4, P5) and control board (SCL, SDA) are connected throught I2C protocal. + +* Laptop connects to OpenMV Camera and Control board (Update the excuation codes) + +* Battery * 3 for control board (1500 mAh), ControlBoard also mount to the Motor Shield + +* **High Level Logics** (Current): + + ```pseudocode + If [Camera] Detect Target --> (x,y): + [Control_Board] do PID control + until move towards (x,y) + Else [Camera] NOT Detect + [Control_Board] Seeking: + Horizontally Rotating + Vertically No Action + ``` + + + +## (Color) Detection Module + +In `feather_main_personaldemo_PID.ino`, in the `loop{...}`, the "If Detect/Not Detect Parts" has an condition called **`cam.exe_color_detection_biggestblob()`** + +```c + ... + if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){...} + else {...}//seeking algorithm + ... +``` + + + +This function **`cam.exe_color_detection_biggestblob()`** can be seen in the file `"Camera.cpp"`, in which , there is + +```c +bool Camera::exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max, int& x, int&y){ + int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max}; + struct { uint16_t cx, cy; } color_detection_result; + if (interface->call(F("color_detection_single"), color_thresholds, sizeof(color_thresholds), &color_detection_result, sizeof(color_detection_result))) { + + } + x = color_detection_result.cx; + y = color_detection_result.cy; + if (x == 0 && y == 0){ + return false; + } else { + return true; + } +} + +``` + + + +It use the `interface->call()` of OpenMV MicroPython [`rpc` library](https://docs.openmv.io/library/omv.rpc.html) function to call the function `color_detection_single` in **Shahrul Kamil bin Hassan / blimp-autonomy / Codes / OpenMV H7 Plus Camera /[openMV_main.py](https://git.uclalemur.com/shahrulkamil98/blimp-autonomy/-/blob/main/Codes/OpenMV H7 Plus Camera/openMV_main.py)** + +```python +# When called returns the x/y centroid of the largest blob +# within the OpenMV Cam's field-of-view. +# +# data is the 6-byte color tracking threshold tuple of L_MIN, L_MAX, A_MIN, A_MAX, B_MIN, B_MAX. +def color_detection_single(data): + red_led.off() + green_led.on() + sensor.set_pixformat(sensor.RGB565) + sensor.set_framesize(sensor.QVGA) + thresholds = struct.unpack("<bbbbbb", data) + print(thresholds) + blobs = sensor.snapshot().find_blobs([thresholds], + pixels_threshold=500, + area_threshold=500, + merge=True, + margin=20) + if not blobs: return struct.pack("<HH", 0, 0)# No detections. + for b in blobs: + sensor.get_fb().draw_rectangle(b.rect(), color = (255, 0, 0)) + sensor.get_fb().draw_cross(b.cx(), b.cy(), color = (0, 255, 0)) + out_blob = max(blobs, key = lambda b: b.density()) + red_led.on() + green_led.off() + return struct.pack("<HH", out_blob.cx(), out_blob.cy()) +``` + + + +There is also another funciton called `color_detection (Thresholds)`, which is a complex version of `color_detection_single()`, and it will be able to detect multiple balls. + +Now, we take deeper look into the **`color_detection_single ()`** function, + +* **Input**: 6 `int8_t` number for **thershold** + + * Threshold is in [LAB format for color](http://shutha.org/node/851) + * All the way from `feather_main_personaldemo.ino`, as + * 6-byte color tracking threshold tuple of `L_MIN, L_MAX, A_MIN, A_MAX, B_MIN, B_MAX` + * Used as a cirteria for [`img.find_blobs()`](https://docs.openmv.io/library/omv.image.html#image.image.Image.image.find_blobs) finding a blob + * `get_fb()`: (Get Frame Buffer) Returns the image object returned by a previous call of `sensor.snapshot()` + * `out_blob`: is the max bolob by density + +* **Output**: Return the center of the blob `(out_blob.cx, out_blob.cy)` + + + +**Note**: + +* Currently, the threshold is manually set to be suitable for detecting green inside the experimenting space. + +* We will upgrade this to ML procdess to have a accurate + + + +## PID Control Module + +### PID Prelim + +$$ +\begin{aligned} +&u(t)=K_{p} e(t)+K_{i} \int e(t) d t+K_{p} \frac{d e}{d t} \\ +&u(t)=\text { PID control variable } \\ +&K_{p}=\text { proportional gain } \\ +&e(t)=\text { error value } \\ +&K_{i}=\text { integral gain } \\ +&d e \quad=\text { change in error value } \\ +&d t=\text { change in time } +\end{aligned} +$$ + +In discrete domain: + +* Integration as summaton +* Derviative as difference + +### Feather PID + +**Shahrul Kamil bin Hassan / november-2021-blimp-competition / Code / Main Feather Code / [feather_main_personaldemo_PID.ino](https://git.uclalemur.com/shahrulkamil98/november-2021-blimp-competition/-/blob/main/Code/Main Feather Code/feather_main_personaldemo_PID.ino)** + +The setup for PID is + +```C +//Specify the links and initial tuning parameters +double Kpx=2, Kix=0.05, Kdx=0.25; +double Kpy=2, Kiy=0.1, Kdy=0.25; +PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT); +PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT); +... +PID_y.SetOutputLimits(-255, 255); //up positive +PID_x.SetOutputLimits(-255, 255); //left positive +``` + +PID is an object class in the library: [PID_v1](https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.cpp) + + + +In the Loop(), for every detection, the returned value of `x,y` as the center of the `out_blob.cx` and `out_blob.cy` + +```c + int x = 0; + int y = 0; + panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby + + if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){ + if (displayTracking > 0){ + displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK + } + panel.singleLED(DEBUG_STATE, 0, 10, 0); + Serial.println("blob detected"); + Serial.print("x value: "); + Serial.println(x); + Serial.print("y value: "); + Serial.println(y); + Inputx = x/1.00; + Inputy = y/1.00; + PID_x.Compute(); // <==== LOOK HERE + PID_y.Compute(); // <==== LOOK HERE + + Serial.println(Outputy); + Serial.println(Outputx); + + //actuate the vertical motor + moveVertical(Outputy); + moveHorizontal(Outputx, BASE_SPEED); + + } else { //seeking algorithm + //panel.resetPanel(); + panel.singleLED(DEBUG_STATE, 10, 10, 10); + Serial.println("seeking..."); + int zero = 0; + moveVertical(zero); + moveHorizontal(SEEKING_SPEED, zero); + } +} + +``` + +`PID_x.Compute()` and `PID_y.Compute()` take the input of `InputX` `InputY` and output `OutputX` `OutputY` + + `OutputX` `OutputY` are in between -255 to 255 + +And pass into the `moveVertical(Outputy)` and`moveHorizontal(Outputx, BASE_SPEED)` to tune the speed + +```c +void moveVertical(int vel){ + if (vel > 0) { //up + panel.singleLED(DEBUG_VERTICALSPEED, abs(vel), 0, 0); + motorVertical->setSpeed(abs((int) vel)); + motorVertical->run(BACKWARD); + } else if(vel < 0) { //down + panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel)); + motorVertical->setSpeed(abs((int) Outputy)); + motorVertical->run(FORWARD); + } else { + panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0); + motorVertical->setSpeed(0); + } +} + +void moveHorizontal(int vel_hori,int base_speed){ + int lspeed = -1*vel_hori + base_speed; + int rspeed = vel_hori + base_speed; + + if (rspeed > 0){ + motorLeft->run(BACKWARD); + } else { + motorLeft->run(FORWARD); + } + + if (lspeed > 0){ + motorRight->run(BACKWARD); + } else { + motorRight->run(FORWARD); + } + displaySpeed(lspeed, rspeed); + motorLeft->setSpeed(min(MAX_SPEED, abs(rspeed))); + motorRight->setSpeed(min(MAX_SPEED, abs(lspeed))); +} +``` + +* For vertical movement, + * -255: DOWN + * 0: Stand Still + * 255: UP +* For horizontal movement, there is a base speed. + * (Suppose BACKWARD really mean backward in the function) + * -255: left = 255+base_speed > 0 ; right = -255+base_speed < 0 (lspeed > 0 rotation to right) + * 0: left = base_speed ; right = base_speed (moving forward without rotation) + * 255: left = -255+base_speed <0 ; right = 255+base_speed > 0 (rspeed > p rotation to left) + + + +## Aspects of Improvement + +***Blue text on the board*** + +1. Replace camera with more sensor + * 2 camera + * IMU + * IR (to determine weather has catched a ball or not) + * Lidar + * Baraometer +2. Color Detection --> upgrade to use ML +3. If Dectect, return a varable `Area` (as the area of the blob in screen shot) in addition to `x` and `y`. +4. PID constant tuning +5. Dynamic Mapping + * Base Speed (Dynamically maps to `Area`) + * Motor (Clockwise/Counter Clockwise) automatically maps to the CORRECT definition of BACKWARD and FORWARD +6. Seeking: Add Vertial Actions + +## Long Term System Setups + +**High Level Logics** (Current): + +```pseudocode +If [Camera1] Detect + 1. Detect AprilTag / Hoop: + [Control_Board] Distinguish Directions + 2. Detect Green Bolloon: + if [IR Sensor] Detects Catch + [Camera2] Seeking / Detect AprilTag or Hoop + Goal: Win Points + Else [IR Sensor] Detect NOT Catch + [Control_Board] do PID control + until Catch The Green Bolloon +Else [Camera1] NOT Detect + [Control_Board] Seeking: + Horizontally Rotating + Vertically Moving +``` + + + + + + + + + + + + + + + + + + + + +