Skip to content

Commit 751ad3d

Browse files
previprevi
committed
pid wip
1 parent 9b7e449 commit 751ad3d

File tree

11 files changed

+57
-103
lines changed

11 files changed

+57
-103
lines changed

‎coderbot.py

Lines changed: 2 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -133,7 +133,6 @@ def __init__(self, motor_trim_factor=1.0, encoder=True):
133133
self._servos = [self.GPIOS.PIN_SERVO_1, self.GPIOS.PIN_SERVO_2]
134134

135135
self.stop()
136-
self._is_moving = False
137136

138137
the_bot = None
139138

@@ -217,15 +216,10 @@ def _servo_control(self, pin, angle):
217216
self.pi.set_PWM_dutycycle(pin, duty)
218217

219218
def stop(self):
220-
if self._encoder:
221-
self._twin_motors_enc.stop()
222-
else:
223-
for pin in self._pin_out:
224-
self.pi.write(pin, 0)
225-
self._is_moving = False
219+
self._twin_motors_enc.stop()
226220

227221
def is_moving(self):
228-
return self._is_moving
222+
return self._twin_motors_enc._is_moving
229223

230224
# Distance travelled getter
231225
def distance(self):

‎data/program_accel_shock_test.json

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
{"name": "accel_shock_test", "dom_code": "<xml xmlns=\"https://developers.google.com/blockly/xml\"><variables><variable id=\"!,+:pDZJ5x.rKnypHXSN\">accel_x</variable><variable id=\"eq6b@E=pMQcsp)8.4B~!\">accel_y</variable></variables><block type=\"controls_whileUntil\" id=\"0Mx[mSKoV~Gk1qbv5Wrn\" x=\"131\" y=\"14\"><field name=\"MODE\">WHILE</field><value name=\"BOOL\"><block type=\"logic_boolean\" id=\"q8(wB97}Y)d71cgy$yo#\"><field name=\"BOOL\">TRUE</field></block></value><statement name=\"DO\"><block type=\"variables_set\" id=\"es98X$l#g7`}kO0x@V_6\"><field name=\"VAR\" id=\"!,+:pDZJ5x.rKnypHXSN\">accel_x</field><value name=\"VALUE\"><block type=\"coderbot_mpu_get_accel\" id=\"3EuN{WmvwR?#dXrw00R~\"><field name=\"AXIS\">0</field></block></value><next><block type=\"variables_set\" id=\"mI%0vQDYGZ{5#L)jDHqZ\"><field name=\"VAR\" id=\"eq6b@E=pMQcsp)8.4B~!\">accel_y</field><value name=\"VALUE\"><block type=\"coderbot_mpu_get_accel\" id=\"Hg-)JdpFT,T:@_Yunr(N\"><field name=\"AXIS\">1</field></block></value><next><block type=\"text_print\" id=\"0fGi]QOF@I*b$5x^?f#A\"><value name=\"TEXT\"><block type=\"text_join\" id=\"cnFZfY3ovofCn5@(@?-)\"><mutation items=\"4\"></mutation><value name=\"ADD0\"><block type=\"text\" id=\"#uYzT9*sMp/H#]n3DiE2\"><field name=\"TEXT\">accel x:</field></block></value><value name=\"ADD1\"><block type=\"variables_get\" id=\"~$])@+:L,KJJ]s9O|yMR\"><field name=\"VAR\" id=\"!,+:pDZJ5x.rKnypHXSN\">accel_x</field></block></value><value name=\"ADD2\"><block type=\"text\" id=\"_}$vf[|[fK@m;/UO!dDa\"><field name=\"TEXT\">accel y:</field></block></value><value name=\"ADD3\"><block type=\"variables_get\" id=\"+h]N2:RoURWutT;t/(^Q\"><field name=\"VAR\" id=\"eq6b@E=pMQcsp)8.4B~!\">accel_y</field></block></value></block></value><next><block type=\"controls_if\" id=\"wbaaU.9gv8EFF)Q1spdt\"><mutation elseif=\"3\"></mutation><value name=\"IF0\"><block type=\"logic_compare\" id=\"NR2RqzMqE#`/ho=`X=~b\"><field name=\"OP\">GT</field><value name=\"A\"><block type=\"variables_get\" id=\"AulTa(a5.NMjxq.ru]z0\"><field name=\"VAR\" id=\"!,+:pDZJ5x.rKnypHXSN\">accel_x</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"mg=hvcJ;5^~:?I2Bq6|/\"><field name=\"NUM\">0.1</field></block></value></block></value><statement name=\"DO0\"><block type=\"coderbot_adv_move\" id=\"sG08`UJh7dHK}v:srjS$\"><field name=\"ACTION\">BACKWARD</field><value name=\"SPEED\"><block type=\"math_number\" id=\"*^tW?[tJKxW6SCnNwNBh\"><field name=\"NUM\">100</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\";f_j}f+NNfdy5?6|,zJ_\"><field name=\"NUM\">0.1</field></block></value><next><block type=\"coderbot_sleep\" id=\"4+FS8;?zx{r0Tz$gt$R]\"><value name=\"ELAPSE\"><block type=\"math_number\" id=\"kMN*!M7n)~Jtx|=@sd;D\"><field name=\"NUM\">0.1</field></block></value></block></next></block></statement><value name=\"IF1\"><block type=\"logic_compare\" id=\"=WsKDp]?}EhI%ZG2~,LF\"><field name=\"OP\">LT</field><value name=\"A\"><block type=\"variables_get\" id=\"Fo|2^:Ck5:}s#3#J+6oB\"><field name=\"VAR\" id=\"!,+:pDZJ5x.rKnypHXSN\">accel_x</field></block></value><value name=\"B\"><block type=\"math_number\" id=\".qktB{g/gqnIX(WH0V.a\"><field name=\"NUM\">-0.1</field></block></value></block></value><statement name=\"DO1\"><block type=\"coderbot_adv_move\" id=\"Q.HJe=q1+dR@DZ_O?KPI\"><field name=\"ACTION\">FORWARD</field><value name=\"SPEED\"><block type=\"math_number\" id=\";rvcfRMHCegkk+tZC3ZT\"><field name=\"NUM\">100</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\"eWGH2b$BSuo+`6Iu=e${\"><field name=\"NUM\">0.1</field></block></value><next><block type=\"coderbot_sleep\" id=\"b9ce`Ci.v{eh3#;IE)!u\"><value name=\"ELAPSE\"><block type=\"math_number\" id=\":Mgy#DS~fLzb[i.@J=$s\"><field name=\"NUM\">0.1</field></block></value></block></next></block></statement><value name=\"IF2\"><block type=\"logic_compare\" id=\"k[I|Syhf63p987*AU;NK\"><field name=\"OP\">GT</field><value name=\"A\"><block type=\"variables_get\" id=\"-?`Mi$Io$Ir:J}9$NY*k\"><field name=\"VAR\" id=\"eq6b@E=pMQcsp)8.4B~!\">accel_y</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"NhL89=)+q,xUsMlY~xVj\"><field name=\"NUM\">0.1</field></block></value></block></value><statement name=\"DO2\"><block type=\"coderbot_adv_move\" id=\"w^i.n,G2cX-z_YHBGJos\"><field name=\"ACTION\">RIGHT</field><value name=\"SPEED\"><block type=\"math_number\" id=\"So|G,tTm%u0!Wy%O#-,4\"><field name=\"NUM\">100</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\"E@Ut?:hT~##lJOHwNt=(\"><field name=\"NUM\">0.1</field></block></value><next><block type=\"coderbot_sleep\" id=\"0w^d8W$h#oGW=9(O+e]f\"><value name=\"ELAPSE\"><block type=\"math_number\" id=\"WGK|Hq9e*o5_|!J@GK(.\"><field name=\"NUM\">0.1</field></block></value></block></next></block></statement><value name=\"IF3\"><block type=\"logic_compare\" id=\"[!x`-oK(i^AUz.s.|-`L\"><field name=\"OP\">LT</field><value name=\"A\"><block type=\"variables_get\" id=\"f:6H;#%4E3=$.qWL(#]Q\"><field name=\"VAR\" id=\"eq6b@E=pMQcsp)8.4B~!\">accel_y</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"0$d?@sUe-0q21Ndbe0i%\"><field name=\"NUM\">-0.1</field></block></value></block></value><statement name=\"DO3\"><block type=\"coderbot_adv_move\" id=\"ob:#4vA%n6@=0?No-a`p\"><field name=\"ACTION\">LEFT</field><value name=\"SPEED\"><block type=\"math_number\" id=\"~U4wI[l*;;/mF{U0Q9Wl\"><field name=\"NUM\">100</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\";txsLo=2p9C6hidfrXO8\"><field name=\"NUM\">0.1</field></block></value><next><block type=\"coderbot_sleep\" id=\"@9u6yx1NymDI2DZB5Exn\"><value name=\"ELAPSE\"><block type=\"math_number\" id=\"(MQ(*9fJa1S?@BSo@OjR\"><field name=\"NUM\">0.1</field></block></value></block></next></block></statement></block></next></block></next></block></next></block></statement></block></xml>", "code": "accel_x = None\naccel_y = None\n\n\nwhile True:\n get_prog_eng().check_end()\n accel_x = get_bot().get_mpu_accel(0)\n accel_y = get_bot().get_mpu_accel(1)\n get_cam().set_text(''.join([str(x) for x in ['accel x:', accel_x, 'accel y:', accel_y]]))\n if accel_x > 0.1:\n get_bot().backward(speed=100, elapse=0.1)\n get_bot().sleep(0.1)\n elif accel_x < -0.1:\n get_bot().forward(speed=100, elapse=0.1)\n get_bot().sleep(0.1)\n elif accel_y > 0.1:\n get_bot().right(speed=100, elapse=0.1)\n get_bot().sleep(0.1)\n elif accel_y < -0.1:\n get_bot().left(speed=100, elapse=0.1)\n get_bot().sleep(0.1)\n", "default": false}

‎data/program_balance_bot.json

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
{"name": "balance_bot", "dom_code": "<xml xmlns=\"https://developers.google.com/blockly/xml\"><variables><variable id=\"8;52SRAV5TdSZddfwY^b\">accel_z</variable></variables><block type=\"variables_set\" id=\"p~y8!j:@_)do.cH@NcZ;\" x=\"173\" y=\"10\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">accel_z</field><value name=\"VALUE\"><block type=\"math_number\" id=\"`gyKeyCP5ks_]J3{Wr{D\"><field name=\"NUM\">0</field></block></value><next><block type=\"controls_whileUntil\" id=\"0Mx[mSKoV~Gk1qbv5Wrn\"><field name=\"MODE\">WHILE</field><value name=\"BOOL\"><block type=\"logic_boolean\" id=\"q8(wB97}Y)d71cgy$yo#\"><field name=\"BOOL\">TRUE</field></block></value><statement name=\"DO\"><block type=\"variables_set\" id=\"es98X$l#g7`}kO0x@V_6\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">accel_z</field><value name=\"VALUE\"><block type=\"math_arithmetic\" id=\"O_oAqn94-nyY,F`E%3_Z\"><field name=\"OP\">ADD</field><value name=\"A\"><block type=\"variables_get\" id=\"w*b)+kp|s3Ih5dNed^m5\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">accel_z</field></block></value><value name=\"B\"><block type=\"math_arithmetic\" id=\"!8`P*.0UKwInKjDY/OS7\"><field name=\"OP\">DIVIDE</field><value name=\"A\"><block type=\"math_arithmetic\" id=\"L.WGI+_s?g+S$SP2c=kR\"><field name=\"OP\">MINUS</field><value name=\"A\"><block type=\"coderbot_mpu_get_accel\" id=\"3EuN{WmvwR?#dXrw00R~\"><field name=\"AXIS\">2</field></block></value><value name=\"B\"><block type=\"variables_get\" id=\"Bd;drEt;1zeS4#!j=zbx\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">accel_z</field></block></value></block></value><value name=\"B\"><block type=\"math_number\" id=\"io]K))_n4OCp^wI1,hd6\"><field name=\"NUM\">2</field></block></value></block></value></block></value><next><block type=\"text_print\" id=\"0fGi]QOF@I*b$5x^?f#A\"><value name=\"TEXT\"><block type=\"variables_get\" id=\"~$])@+:L,KJJ]s9O|yMR\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">accel_z</field></block></value><next><block type=\"controls_if\" id=\"wbaaU.9gv8EFF)Q1spdt\"><mutation elseif=\"1\" else=\"1\"></mutation><value name=\"IF0\"><block type=\"logic_compare\" id=\"NR2RqzMqE#`/ho=`X=~b\"><field name=\"OP\">LT</field><value name=\"A\"><block type=\"variables_get\" id=\"AulTa(a5.NMjxq.ru]z0\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">accel_z</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"mg=hvcJ;5^~:?I2Bq6|/\"><field name=\"NUM\">0.63</field></block></value></block></value><statement name=\"DO0\"><block type=\"coderbot_adv_move\" id=\"sG08`UJh7dHK}v:srjS$\"><field name=\"ACTION\">FORWARD</field><value name=\"SPEED\"><block type=\"math_number\" id=\"*^tW?[tJKxW6SCnNwNBh\"><field name=\"NUM\">100</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\";f_j}f+NNfdy5?6|,zJ_\"><field name=\"NUM\">-1</field></block></value></block></statement><value name=\"IF1\"><block type=\"logic_compare\" id=\"=WsKDp]?}EhI%ZG2~,LF\"><field name=\"OP\">GT</field><value name=\"A\"><block type=\"variables_get\" id=\"Fo|2^:Ck5:}s#3#J+6oB\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">accel_z</field></block></value><value name=\"B\"><block type=\"math_number\" id=\".qktB{g/gqnIX(WH0V.a\"><field name=\"NUM\">0.62</field></block></value></block></value><statement name=\"DO1\"><block type=\"coderbot_adv_move\" id=\"Q.HJe=q1+dR@DZ_O?KPI\"><field name=\"ACTION\">BACKWARD</field><value name=\"SPEED\"><block type=\"math_number\" id=\";rvcfRMHCegkk+tZC3ZT\"><field name=\"NUM\">100</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\"eWGH2b$BSuo+`6Iu=e${\"><field name=\"NUM\">-1</field></block></value></block></statement><statement name=\"ELSE\"><block type=\"coderbot_adv_stop\" id=\"4R~Ix)C[^,2GTB5SLR9p\"></block></statement></block></next></block></next></block></statement></block></next></block></xml>", "code": "accel_z = None\n\n\naccel_z = 0\nwhile True:\n get_prog_eng().check_end()\n accel_z = accel_z + (get_bot().get_mpu_accel(2) - accel_z) / 2\n get_cam().set_text(accel_z)\n if accel_z < 0.63:\n get_bot().forward(speed=100, elapse=-1)\n elif accel_z > 0.62:\n get_bot().backward(speed=100, elapse=-1)\n else:\n get_bot().stop()\n", "default": false}

‎data/program_gyro_test.json

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
{"name": "gyro_test", "dom_code": "<xml xmlns=\"https://developers.google.com/blockly/xml\"><variables><variable id=\"8;52SRAV5TdSZddfwY^b\">gyro_z</variable></variables><block type=\"variables_set\" id=\"EQena`d(B:Ed+dYrN5-g\" x=\"161\" y=\"52\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">gyro_z</field><value name=\"VALUE\"><block type=\"math_number\" id=\"J+;SGOnbR10];lf*4Y_=\"><field name=\"NUM\">0</field></block></value><next><block type=\"controls_whileUntil\" id=\"0Mx[mSKoV~Gk1qbv5Wrn\"><field name=\"MODE\">WHILE</field><value name=\"BOOL\"><block type=\"logic_boolean\" id=\"q8(wB97}Y)d71cgy$yo#\"><field name=\"BOOL\">TRUE</field></block></value><statement name=\"DO\"><block type=\"math_change\" id=\"n~dzYckR~r]~x5`8:hm%\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">gyro_z</field><value name=\"DELTA\"><shadow type=\"math_number\" id=\"dT*fy;4R5SaeE8.dq=:(\"><field name=\"NUM\">1</field></shadow><block type=\"math_arithmetic\" id=\"0wi`nqfBX_*O$KzqG@#R\"><field name=\"OP\">DIVIDE</field><value name=\"A\"><block type=\"coderbot_mpu_get_gyro\" id=\"EJ*MTRj:Bq;3Qn+.jHnU\"><field name=\"AXIS\">2</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"ZWqd^`^.8tm1?fB4bz=A\"><field name=\"NUM\">10</field></block></value></block></value><next><block type=\"text_print\" id=\"0fGi]QOF@I*b$5x^?f#A\"><value name=\"TEXT\"><block type=\"variables_get\" id=\"~$])@+:L,KJJ]s9O|yMR\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">gyro_z</field></block></value><next><block type=\"controls_if\" id=\"wbaaU.9gv8EFF)Q1spdt\"><mutation else=\"1\"></mutation><value name=\"IF0\"><block type=\"logic_compare\" id=\"NR2RqzMqE#`/ho=`X=~b\"><field name=\"OP\">LT</field><value name=\"A\"><block type=\"variables_get\" id=\"AulTa(a5.NMjxq.ru]z0\"><field name=\"VAR\" id=\"8;52SRAV5TdSZddfwY^b\">gyro_z</field></block></value><value name=\"B\"><block type=\"math_number\" id=\"mg=hvcJ;5^~:?I2Bq6|/\"><field name=\"NUM\">90</field></block></value></block></value><statement name=\"DO0\"><block type=\"coderbot_adv_move\" id=\"sG08`UJh7dHK}v:srjS$\"><field name=\"ACTION\">LEFT</field><value name=\"SPEED\"><block type=\"math_number\" id=\"*^tW?[tJKxW6SCnNwNBh\"><field name=\"NUM\">100</field></block></value><value name=\"ELAPSE\"><block type=\"math_number\" id=\";f_j}f+NNfdy5?6|,zJ_\"><field name=\"NUM\">0.01</field></block></value></block></statement><statement name=\"ELSE\"><block type=\"coderbot_adv_stop\" id=\"4R~Ix)C[^,2GTB5SLR9p\"></block></statement></block></next></block></next></block></statement></block></next></block></xml>", "code": "from numbers import Number\n\ngyro_z = None\n\n\ngyro_z = 0\nwhile True:\n get_prog_eng().check_end()\n gyro_z = (gyro_z if isinstance(gyro_z, Number) else 0) + get_bot().get_mpu_gyro(2) / 10\n get_cam().set_text(gyro_z)\n if gyro_z < 90:\n get_bot().left(speed=100, elapse=0.01)\n else:\n get_bot().stop()\n", "default": false}

‎data/programs.json

Lines changed: 1 addition & 1 deletion
Large diffs are not rendered by default.

‎program.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -205,6 +205,7 @@ def run(self):
205205
except Exception:
206206
logging.error("error polishing event system")
207207
try:
208+
get_bot().stop()
208209
get_cam().video_stop() #if video is running, stop it
209210
get_cam().set_text("") #clear overlay text (if any)
210211
get_motion().stop()

0 commit comments

Comments
 (0)