fl { PropForth 5.5 2022/11/06 18:50:11 Motor power | ----------- P0 ------|step 4988A|---------stepping motorX P1 ------|dir | 4wires ---------- Fullstep Motor power | ---------- P2 ------|step 4988A|---------stepping motoyY P3 ------|dir | 4wires ---------- Fullstep Servo SG90 --- P4 ---orange-|PWM| 5V----red----|Vcc| GND---black--|GND| --- Propeler ------------ ------------- | Prpcessing |-------|USB-serial RX| --- P5 | 3 | | TX| --- P6 ------------ ------------- } \ ******************* \ Constants \ ******************* \ ------- stepping motor ---------------- 0 wconstant stepX 1 wconstant dirX 2 wconstant stepY 3 wconstant dirY 4 wconstant servo \ Special register h1F8 wconstant ctra h1FA wconstant frqa h1FC wconstant phsa d1600000 constant 20msec d192000 constant 2.4msec d160000 constant 2.0msec d120000 constant 1.5msec d40000 constant 0.5msec \ serial for PropForth<-->OpenLog 5 wconstant Rx 6 wconstant Tx d2400 constant baudrate \ 9600/4 : allCorrect c" ok" ; : str1 c" G1" ; : str2 c" G4" ; : str3 c" M300" ; : str4 c" S30" ; : str5 c" S50" ; \ calculating float h42C8_0000 constant f100 0 wconstant stepDelay d50 wconstant lineDelay d50 wconstant stepDelay d667 wconstant stepPerMilimeterX \ 6.67step/mm * 100 d714 wconstant stepPerMilimeterY \ 7.14step/mm * 100 0 wconstant Xmin d3900 wconstant Xmax 0 wconstant Ymin d3500 wconstant Ymax \ ******************* \ Variables \ ******************* \ ------- stepping motor ---------------- variable curPosX variable curPosY variable dif1 variable dif2 variable servoPos variable count \ ---- serial --------- wvariable inchar wvariable lineSemiColon wvariable lineIsComment variable working 4 allot wvariable dummy \ ----- draw ----------- variable newPos.x variable newPos.y variable xPos variable yPos variable dx variable dy variable error variable sx variable sy variable x1 variable y1 \ ******************* \ Main \ ******************* \ Start up serial-communication \ This need to execute when staring serial-communication at first \ ( -- ) : initSerial c" Tx Rx baudrate serial" 4 cogx \ Start serial on cog4 d100 delms inchar 4 cogio 2+ W! \ Set output of cog4 inchar h100 inchar W! \ Clear inchar 1 4 sersetflags ; \ Stop Serial communication \ This need to execute when finishing serial-communication at last \ ( -- ) : stopSerial 0 4 cogio 2+ W! 4 cogreset ; \ Transmit data[1byte] \ ( n1 -- ) n1:transmitting byte : Transmit begin 4 cogio W@ h100 and until \ Wait until input for serial-cog is under ready-state 4 cogio W! \ Write data to cog4's input ; \ Receive data and save them in free area \ ( n1 -- n2 ) n1:repeat number n2:last addres+1 of free area : Receive here W@ swap 0 do begin inchar W@ h100 and 0= until \ Wait until output for serial-cog is under ready-state inchar W@ over C! \ Save output-data of cog4 to free area h100 inchar W! \ Clear inchar1 1+ \ Increment free space address loop ; \ Tranmit string \ ( n1 -- ) : sendStr dup C@ 0 do 1+ dup C@ Transmit loop drop ; { \ Receiving all data from Processing : allReceive initSerial h6F Transmit h6B Transmit here W@ begin inchar W@ h100 and 0= if inchar W@ over C! inchar W@ dup emit dup d10 = swap d13 = or if allCorrect sendStr d10 Transmit then h100 inchar W! \ Clear inchar 1+ then fkey? swap drop until stopSerial drop ; } \ ---- stepping motor ---------- \ Set L/H to dir1/dir2 \ ( -- ) : stepX_L 0 stepX px ; : stepX_H 1 stepX px ; : stepY_L 0 stepY px ; : stepY_H 1 stepY px ; \ ( n1 -- ) n1:0 left->right 1 left<-right : stepperX dup dirX px stepX_H 10 delms stepX_L d10 delms 0= if curPosX L@ 1+ else curPosX L@ 1- then curPosX L! ; \ ( n1 -- ) n1:0 hi->lo 1 lo->hi : stepperY dup dirY px stepY_H 20 delms stepY_L d20 delms 0= if curPosY L@ 1+ else curPosY L@ 1- then curPosY L! ; { \ stepper direction TEST \ ( -- ) : x- 100 0 do 1 stepperX loop ; : x+ 100 0 do 0 stepperX loop ; : y- 100 0 do 1 stepperY loop ; : y+ 100 0 do 0 stepperY loop ; } \ ------ servo ------------------- \ Save servo position \ ( n1 -- ) n1:servo pulse width : setPos servoPos L! ; \ Runs servos \ ( -- ) : servoDrv \ Set PWM/NCO mode on servo pin servo h10000000 or ctra COG! 1 frqa COG! 0 phsa COG! servo pinout 1.5msec setPos \ initial position 20msec cnt COG@ + \ cnt + 20msec begin \ Set negative value to phsa servoPos L@ dup . cr negate phsa COG! \ 2.4msec waitcnt drop 20msec waitcnt \ cnt + 20msec 0 until \ fkey? swap drop \ until \ drop ; \ pen up/down : penUp 2.0msec setPos ; : penDown 1.5msec setPos ; \ ------------------------------------------------- \ Draw line of (newPos.x,newPos.y)-(x1,y1) \ ( n1 n2 -- ) n1:x n2:y : drawLine dup Ymax >= if drop Ymax then swap dup Xmax >= if drop Xmax then \ ( y x ) stepPerMilimeterX * x1 L! stepPerMilimeterY * y1 L! \ ( -- ) x1 L@ xPos L@ - abs dx L! y1 L@ yPos L@ - abs dy L! dx L@ d10000 / dx L! dy L@ d10000 / dy L! \ ." dx:" dx L@ . space ." dy:" dy L@ . cr xPos L@ x1 L@ < if 0 sx L! else 1 sx L! then yPos L@ y1 L@ < if 0 sy L! else 1 sy L! then 0 error L! dx L@ dy L@ > if dx L@ 0 do sx L@ stepperX \ ." sx1:" sx L@ . space error L@ dy L@ + error L! error L@ dx L@ >= if error L@ dx L@ - error L! sy L@ stepperY \ ." sy1:" sy L@ . space then stepDelay delms loop else dy L@ 0 do sy L@ stepperY \ ." sy2:" sy L@ . space error L@ dx L@ + error L! error L@ dy L@ >= if error L@ dy L@ - error L! sx L@ stepperX \ ." sx2:" sx L@ . space then stepDelay delms loop then \ cr curPosX L@ . curPosY L@ . cr \ cr x1 L@ xPos L! y1 L@ yPos L! ; \ Search space \ ( n1 -- n2 ) n1:start address n2:addres+1 of searched space from n1 : searchSpace begin dup C@ h20 = if 1 else 0 then swap 1+ swap until ; \ string copied from here W@(string's separate is h20) \ working's 1st-byte is its string-length \ Copy strings to add str-length from [here W@] to working \ ( n1 -- ) n1:copied string's address : nameCopy \ Get 1st-string's length dup searchSpace over - 1- working C! \ Copy string working 1+ swap working C@ 0 do 2dup i + C@ swap i + C! loop 2drop ; \ 71 here W@ C! 49 here W@ 1+ C! 32 here W@ 2+ C! \ G1 copy to here W@ \ h53 here W@ 3 + C! h33 here W@ 4 + C! h30 here W@ 5 + C! h20 here W@ 6 + C! \ Get value of XorY from working \ ( -- ) : getValue working C@ 5 = \ 5characters [example Y1.14] if \ Shift string to right 1char by 1char \ [working+5,4,3,2 --> working+6,5,4,3] 5 4 0 do dup i - working + C@ over 1+ i - working + C! loop drop h30 working 2+ C! \ 6characters [example Y01.14] then \ Convert charcter-number to value working 2+ C@ h30 - d1000 * working 3 + C@ h30 - d100 * + working 5 + C@ h30 - d10 * + working 6 + C@ h30 - + ; \ Get values(X Y) from strings(X Y) \ ( -- ) : getXY here W@ 5 + C@ h20 <> \ 'G1 X0 Y0' if here W@ 3 + C@ h58 = \ If not X(h58), break loop if \ Converting X-value here W@ 4+ C@ h2D = \ Check if '-' if 0 newPos.x L! \ X \ Search Y here W@ 3 + searchSpace 1+ dup C@ h2D = \ Check if '-' if drop 0 else nameCopy getValue then newPos.y L! else \ X here W@ searchSpace nameCopy getValue \ working 10 dump newPos.x L! \ Y here W@ searchSpace searchSpace 1+ dup C@ h2D = \ Check if '-' if drop 0 else 1- nameCopy getValue then newPos.y L! then \ ." X :" newPos.x L@ . space ." Y :" newPos.y L@ . newPos.x L@ newPos.y L@ drawLine then then ; \ Execute command \ ( -- ) : processInComing here W@ nameCopy str1 working name= if \ ." G1" \ Get X,Y values getXY else str2 working name= if \ ." G4" \ Get wait time here W@ searchSpace 1+ dup C@ h30 - d100 * over 1+ C@ h30 - d10 * + over 2+ C@ h30 - + delms drop else str3 working name= if \ ." M300" \ pen Up or Down? here W@ searchSpace 1+ dup C@ h30 - d10 * over 1+ C@ h30 - + dup d30 = if drop penDown \ ." pen down" else d50 = if penUp then \ ." pen up" then drop then then then \ cr ; \ Initial setting \ ( -- ) : init c" servoDrv" 0 cogx stepX pinout dirX pinout stepY pinout dirY pinout 0 curPosX L! 0 curPosY L! initSerial 0 dummy W! 0 xPos L! 0 yPos L! penUp ; \ Drawing data from Processing \ ( -- ) : plot init 1 here W@ begin inchar W@ h100 and 0= if inchar W@ dup \ ( buffer_top_aadr n n ) d10 = over d13 = or \ ( buffer_top_aadr n 1/0 ) if over C! \ ( buffer_top_aadr ) swap dup . 1+ swap dup here W@ <> if \ end of line { \ checking for receiving data from Processing here W@ begin dup C@ dup d10 = if drop 1 else emit 0 then swap 1+ swap until drop here W@ 30 dump cr } \ Eliminate first line( because this cannot skip) from Prpcessing dummy W@ if processInComing else 1 dummy W! then drop here W@ \ Back to top ( [here W@] ) then 0 lineIsComment W! allCorrect sendStr d10 Transmit else lineIsComment W@ if dup \ ( buffer_top_aadr n n ) h29 = \ ")" if 0 lineIsComment W! then drop else dup \ ( buffer_top_aadr n n ) h28 = \ "(" if 1 lineIsComment W! drop else over C! \ ( buffer_top_aadr ) 1+ then then then h100 inchar W! \ Clear inchar then fkey? swap drop until stopSerial 0 cogreset 2drop ;