From 75f733660187de66e0eb5753749cf6a19409b211 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=B0=A7=E5=8E=9F=E5=AD=90?= Date: Thu, 15 May 2025 20:30:35 +0800 Subject: [PATCH] 1 --- 文件/hdt.txt | 97 +++++++++++++++++++++++++++ 文件/新建 文本文档.txt | 147 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 244 insertions(+) create mode 100644 文件/hdt.txt create mode 100644 文件/新建 文本文档.txt diff --git a/文件/hdt.txt b/文件/hdt.txt new file mode 100644 index 0000000..19fa966 --- /dev/null +++ b/文件/hdt.txt @@ -0,0 +1,97 @@ +# EYEBOT Name +EYEBOT IBot-D + +# MOTOR Number | TableName +MOTOR 1 Motor_Table +MOTOR 2 Motor_Table + +# Servo Number | Low | High |TableName +SERVO 1 0 255 Servo_Table +SERVO 2 0 255 Servo_Table +SERVO 3 0 255 Servo_Table +SERVO 4 0 255 Servo_Table + +# ENCODER Number | Clicks per meter +ENCODER 1 6900 +ENCODER 2 6900 + +# PSD NUMER | TableName +PSD 1 PSD_TableA +PSD 2 PSD_TableA +PSD 3 PSD_TableA +PSD 4 PSD_TableA + +# WIFI Default_Hotspot(0)/Slave(1)/Custom_Hotspot(2) +WIFI 0 + +# HOTSPOT Network_Name | Password +# HOTSPOT rob rasp + +# SLAVE Network_Name | Password +SLAVE SSID PASS + +# RoBIOS On(1)/Off(0) | FontSize +ROBIOS 1 10 + +# USB Number | DeviceName +USB 1 EyeBot +USB 2 GPS + +# DISPLAY LCD(0)/HDMI(1) | Fullscreen Off/On (0/1) | Rotation (0/1) | Autorefresh (0/1) +# Note: Change of rotation requires 2 reboots +LCD 0 0 0 1 + +# RPI RaspPi_Ver(1/2/3/4) +RPI 4 + +# DRIVE Wheel distance (90 or 140mm) | Max Motor Speed | Motor1/left dir. (0 = c/w) | Motor2/right dir. (1=clockwise) +DRIVE 110 200 1 2 + +#DEMOPATH | Path +DEMOPATH /home/pi/eyebot/demo + +#SOFTWAREPATH | Path +SOFTWAREPATH /home/pi/usr + +# VOMEGA | Kp | Ki | Kd | Klink (Note if 0,0,0,0 will turn off) +# ... PID frequency, Dead-Rockoning min. change for update, Debug-Flag +VOMEGA 400 40 10 50 10 20 0 + +# -------------------- TABLES (Optional) ------------------ +# Motor Linearisation Table 101 values: 0 .. 100 +TABLE Motor_Table + 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 + 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 + 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 + 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 + 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 +100 +END TABLE + +# Servo Linearisation Table 256 values: 0 .. 255 +TABLE Servo_Table + 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 + 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 + 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 + 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 + 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 +100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 +120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 +140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 +160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 +180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 +200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 +220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 +240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 +END TABLE + +# PSD Sensor Linearisation Table 128 values in [mm]: +TABLE PSD_TableA +800 800 800 800 800 800 800 800 +800 800 800 800 800 800 800 800 800 800 800 770 750 720 700 680 650 610 590 560 +540 530 480 470 460 450 440 430 420 410 400 390 380 370 360 350 340 330 330 320 +300 290 280 280 280 270 270 260 250 250 250 240 240 230 230 220 220 210 210 210 +200 200 190 190 190 190 180 180 170 170 170 160 160 160 160 150 150 150 150 150 +140 140 140 140 140 130 130 130 130 130 130 120 120 120 120 110 110 110 110 110 +110 110 110 110 110 110 110 110 110 100 100 100 100 100 100 100 90 90 90 90 +END TABLE diff --git a/文件/新建 文本文档.txt b/文件/新建 文本文档.txt new file mode 100644 index 0000000..27d4871 --- /dev/null +++ b/文件/新建 文本文档.txt @@ -0,0 +1,147 @@ +#include "eyebot.h" +#define SAFE 200 + +static int v_des1,v_des2; + +float Kp=0.25; +float Ki=0.06; +float Klink=0.05; + +TIMER t1; + +void picontroller() +{ + int enc_new1,v_act1,r_mot1,e_func1; + int enc_new2,v_act2,r_mot2,e_func2; + static int enc_old1,enc_old2; + static int r_old1=0,e_old1=0; + static int r_old2=0,e_old2=0; + + enc_new1=ENCODERRead(1); + enc_new2=ENCODERRead(2); + + v_act1=enc_new1-enc_old1; + v_act2=enc_new2-enc_old2; + + e_func1=v_des1-v_act1; + e_func2=v_des2-v_act2; + + + r_mot1=r_old1+Kp*(e_func1-e_old1)+Ki*(e_func1+e_old1)/2+Klink*(abs(enc_new1)-abs(enc_new2)); + r_mot2=r_old2+Kp*(e_func2-e_old2)+Ki*(e_func2+e_old2)/2-Klink*(abs(enc_new1)-abs(enc_new2)); + + if(v_des1>0&&v_des2>0) + { + if(r_mot1>35)r_mot1=35; + else if(r_mot1<0) r_mot1=0; + + if(r_mot2>35)r_mot2=35; + else if(r_mot2<0) r_mot2=0; + } + else if(v_des1>0&&v_des2<0) + { + if(r_mot1>35)r_mot1=35; + else if(r_mot1<0) r_mot1=0; + if(r_mot2>0)r_mot2=0; + else if(r_mot2<-35) r_mot2=-35; + } + else if(v_des1<0&&v_des2>0) + { + if(r_mot1>0)r_mot1=0; + else if(r_mot1<-35) r_mot1=-35; + if(r_mot2>35)r_mot2=35; + else if(r_mot2<0) r_mot2=0; + } + else if(v_des1==0&&v_des2==0) + { + r_mot1=0; + r_mot2=0; + v_act1=0; + v_act2=0; + enc_old1=0; + r_old1=0; + enc_old2=0; + r_old2=0; + e_func1=0; + e_func2=0; + ENCODERReset(1); + ENCODERReset(2); + } + + MOTORDrive(1,r_mot1); + MOTORDrive(2,r_mot2); + + enc_old1=enc_new1; + r_old1=r_mot1; + e_old1=e_func1; + + enc_old2=enc_new2; + r_old2=r_mot2; + e_old2=e_func2; + + printf("%d %d %d %d\n",r_mot1,r_mot2,enc_old1,enc_old2); + +} + +void VWSpeed(int v1,int v2) +{ + v_des1=v1; + v_des2=v2; +} + + +void drive() +{ + do + { + if(PSDGetRaw(PSD_LEFT)SAFE); + VWSpeed(0,0); + OSWait(1000); +} + +void turn() +{ + VWSpeed(50,-50); + while(PSDGetRaw(PSD_FRONT)<400) OSWait(80); + VWSpeed(0,0); + OSWait(1000); +} + +int main() +{ + LCDMenu("STOP","2","3","END"); + ENCODERReset(1); + ENCODERReset(2); + + PSDGetRaw(PSD_FRONT); + PSDGetRaw(PSD_LEFT); + + + t1=OSAttachTimer(100,picontroller); + + while(KEYRead()!=KEY1) + { drive(); + turn(); + } + + OSDetachTimer(t1); + + AUBeep(); + + while(KEYRead()!=KEY4) + { + MOTORDrive(1,0); + MOTORDrive(2,0); + } + + return(0); + } + + +