summaryrefslogtreecommitdiff
path: root/debug/motordriver
diff options
context:
space:
mode:
Diffstat (limited to 'debug/motordriver')
-rw-r--r--debug/motordriver/kiwidrive_calc.svg323
-rw-r--r--debug/motordriver/nomultiplexer/nomultiplexer.ino294
-rw-r--r--debug/motordriver/onemotor/onemotor.ino88
3 files changed, 590 insertions, 115 deletions
diff --git a/debug/motordriver/kiwidrive_calc.svg b/debug/motordriver/kiwidrive_calc.svg
new file mode 100644
index 0000000..3ca09e2
--- /dev/null
+++ b/debug/motordriver/kiwidrive_calc.svg
@@ -0,0 +1,323 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="210mm"
+ height="297mm"
+ viewBox="0 0 210 297"
+ version="1.1"
+ id="svg8"
+ inkscape:version="1.0.2 (e86c870879, 2021-01-15)"
+ sodipodi:docname="kiwidrive_calc.svg">
+ <defs
+ id="defs2" />
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.4"
+ inkscape:cx="350.96101"
+ inkscape:cy="562.71867"
+ inkscape:document-units="mm"
+ inkscape:current-layer="layer1"
+ inkscape:document-rotation="0"
+ showgrid="false"
+ inkscape:window-width="1920"
+ inkscape:window-height="1125"
+ inkscape:window-x="2560"
+ inkscape:window-y="0"
+ inkscape:window-maximized="1" />
+ <metadata
+ id="metadata5">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <ellipse
+ style="fill:#ff0000;fill-rule:evenodd;stroke-width:0.264583"
+ id="path10"
+ cx="99.888367"
+ cy="134.68913"
+ rx="39.564999"
+ ry="38.587662" />
+ <path
+ style="fill:#a0a0a0;stroke:none;stroke-width:0.0944941"
+ d="m 139.72661,146.01799 c -1.63175,-1.14292 -2.20174,-11.72757 -0.90654,-16.83422 0.39799,-1.56917 0.89479,-2.32023 1.34986,-2.04073 0.45996,0.2825 0.99183,1.8708 1.27726,3.81422 0.51922,3.53519 0.43362,9.37384 -0.18105,12.34962 -0.43334,2.09796 -0.99722,3.09095 -1.53953,2.71111 z"
+ id="path22" />
+ <path
+ style="fill:#a0a0a0;stroke:none;stroke-width:0.0944941"
+ d="m 63.718331,112.73542 c -0.31847,-1.96659 6.871434,-9.75535 11.435089,-12.38756 1.402322,-0.808823 2.287091,-0.976383 2.404045,-0.455293 0.1182,0.526683 -0.64948,2.015393 -1.843555,3.575053 -2.17208,2.83712 -6.417053,6.84677 -8.978746,8.48104 -1.806017,1.15218 -2.910982,1.44035 -3.016833,0.78676 z"
+ id="path22-3" />
+ <path
+ style="fill:#a0a0a0;stroke:none;stroke-width:0.0944941"
+ d="m 74.709966,169.73071 c -1.970594,0.29267 -9.664519,-6.99861 -12.236726,-11.59635 -0.790382,-1.4128 -0.946345,-2.29969 -0.423758,-2.4098 0.528179,-0.11131 2.0067,0.67581 3.550588,1.89022 2.808431,2.20905 6.762138,6.50618 8.362712,9.08905 1.12843,1.82096 1.402097,2.9296 0.747184,3.02688 z"
+ id="path22-3-6"
+ inkscape:transform-center-x="37.420481"
+ inkscape:transform-center-y="-21.837511" />
+ <rect
+ style="fill:#0000ff;fill-rule:evenodd;stroke-width:0.264583"
+ id="rect64"
+ width="186.92551"
+ height="1.8174417"
+ x="17.333752"
+ y="135.38728" />
+ <rect
+ style="fill:#0000ff;fill-rule:evenodd;stroke-width:0.264583"
+ id="rect64-7"
+ width="186.92551"
+ height="1.8174417"
+ x="31.72876"
+ y="-100.25121"
+ transform="rotate(89.560511)" />
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="197.80376"
+ y="132.93736"
+ id="text88"><tspan
+ sodipodi:role="line"
+ id="tspan86"
+ x="197.80376"
+ y="132.93736"
+ style="stroke-width:0.264583">x</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="101.31552"
+ y="42.965397"
+ id="text92"><tspan
+ sodipodi:role="line"
+ id="tspan90"
+ x="101.31552"
+ y="42.965397"
+ style="stroke-width:0.264583">y</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="92.677986"
+ y="145.29591"
+ id="text96"><tspan
+ sodipodi:role="line"
+ id="tspan94"
+ x="92.677986"
+ y="145.29591"
+ style="stroke-width:0.264583">0</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="142.35989"
+ y="149.98514"
+ id="text96-5"><tspan
+ sodipodi:role="line"
+ id="tspan94-3"
+ x="142.35989"
+ y="149.98514"
+ style="stroke-width:0.264583">1</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="94.360558"
+ y="101.32148"
+ id="text96-5-5"><tspan
+ sodipodi:role="line"
+ id="tspan94-3-6"
+ x="94.360558"
+ y="101.32148"
+ style="stroke-width:0.264583">1</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="46.688869"
+ y="147.24464"
+ id="text96-5-5-2"><tspan
+ sodipodi:role="line"
+ x="46.688869"
+ y="147.24464"
+ style="stroke-width:0.264583"
+ id="tspan170">-1</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="88.356491"
+ y="186.57739"
+ id="text96-5-5-2-1"><tspan
+ sodipodi:role="line"
+ x="88.356491"
+ y="186.57739"
+ style="stroke-width:0.264583"
+ id="tspan170-2">-1</tspan></text>
+ <circle
+ id="path198"
+ style="fill:#000000;stroke:#000000;stroke-width:0.264583"
+ cx="144.59926"
+ cy="132.06937"
+ r="0.012488274" />
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="140.65088"
+ y="140.43587"
+ id="text202"><tspan
+ sodipodi:role="line"
+ id="tspan200"
+ x="140.65088"
+ y="140.43587"
+ style="font-size:4.23333px;stroke-width:0.264583">0rad</tspan><tspan
+ sodipodi:role="line"
+ x="140.65088"
+ y="153.66498"
+ style="stroke-width:0.264583"
+ id="tspan204" /></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:4.23333px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="59.606441"
+ y="102.25119"
+ id="text208"><tspan
+ sodipodi:role="line"
+ id="tspan206"
+ x="59.606441"
+ y="102.25119"
+ style="font-size:4.23333px;stroke-width:0.264583">2.09rad</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:4.23333px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="53.299278"
+ y="167.29311"
+ id="text212"><tspan
+ sodipodi:role="line"
+ id="tspan210"
+ x="53.299278"
+ y="167.29311"
+ style="font-size:4.23333px;stroke-width:0.264583">4.18rad</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="142.78784"
+ y="135.75781"
+ id="text216"><tspan
+ sodipodi:role="line"
+ id="tspan214"
+ x="142.78784"
+ y="135.75781"
+ style="stroke-width:0.264583">A</tspan><tspan
+ sodipodi:role="line"
+ x="142.78784"
+ y="148.98694"
+ style="stroke-width:0.264583"
+ id="tspan218" /><tspan
+ sodipodi:role="line"
+ x="142.78784"
+ y="162.21606"
+ style="stroke-width:0.264583"
+ id="tspan220" /></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="68.655945"
+ y="112.41339"
+ id="text216-7"><tspan
+ sodipodi:role="line"
+ id="tspan214-0"
+ x="68.655945"
+ y="112.41339"
+ style="stroke-width:0.264583">B</tspan><tspan
+ sodipodi:role="line"
+ x="68.655945"
+ y="125.64252"
+ style="stroke-width:0.264583"
+ id="tspan218-9" /><tspan
+ sodipodi:role="line"
+ x="68.655945"
+ y="138.87164"
+ style="stroke-width:0.264583"
+ id="tspan220-3" /></text>
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:10.5833px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="67.528885"
+ y="164.8815"
+ id="text216-7-6"><tspan
+ sodipodi:role="line"
+ id="tspan214-0-0"
+ x="67.528885"
+ y="164.8815"
+ style="stroke-width:0.264583">C</tspan><tspan
+ sodipodi:role="line"
+ x="67.528885"
+ y="178.11063"
+ style="stroke-width:0.264583"
+ id="tspan298" /><tspan
+ sodipodi:role="line"
+ x="67.528885"
+ y="191.33975"
+ style="stroke-width:0.264583"
+ id="tspan218-9-6" /><tspan
+ sodipodi:role="line"
+ x="67.528885"
+ y="204.56888"
+ style="stroke-width:0.264583"
+ id="tspan220-3-2" /></text>
+ <rect
+ style="fill:#c900ff;fill-opacity:1;fill-rule:evenodd;stroke-width:0.264583"
+ id="rect318"
+ width="40.081783"
+ height="3.5191765"
+ x="-9.3766651"
+ y="166.49748"
+ transform="rotate(-39.971721)" />
+ <rect
+ style="fill:#c900ff;fill-opacity:1;fill-rule:evenodd;stroke-width:0.12328"
+ id="rect318-6"
+ width="27.441587"
+ height="1.1159221"
+ x="104.34172"
+ y="135.77676"
+ transform="matrix(0.99998555,0.0053753,-0.01704694,0.99985469,0,0)" />
+ <rect
+ style="fill:#c900ff;fill-opacity:1;fill-rule:evenodd;stroke-width:0.127056"
+ id="rect318-6-1"
+ width="29.147621"
+ height="1.1159564"
+ x="-135.70338"
+ y="128.97763"
+ transform="matrix(-0.00379001,-0.99999282,0.9999488,0.01011907,0,0)" />
+ <text
+ xml:space="preserve"
+ style="font-style:normal;font-weight:normal;font-size:4.58611px;line-height:1.25;font-family:sans-serif;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.264583"
+ x="102.17614"
+ y="135.32759"
+ id="text208-8"><tspan
+ sodipodi:role="line"
+ id="tspan206-7"
+ x="102.17614"
+ y="135.32759"
+ style="font-size:4.58611px;stroke-width:0.264583">0.52rad</tspan></text>
+ <rect
+ style="fill:#c98b00;fill-opacity:1;fill-rule:evenodd;stroke-width:0.121786"
+ id="rect318-6-1-0"
+ width="26.779816"
+ height="1.1159475"
+ x="-135.58972"
+ y="138.72464"
+ transform="matrix(-0.00412511,-0.99999149,0.99995678,0.00929711,0,0)" />
+ </g>
+</svg>
diff --git a/debug/motordriver/nomultiplexer/nomultiplexer.ino b/debug/motordriver/nomultiplexer/nomultiplexer.ino
index d2800a8..4ca1ca8 100644
--- a/debug/motordriver/nomultiplexer/nomultiplexer.ino
+++ b/debug/motordriver/nomultiplexer/nomultiplexer.ino
@@ -2,25 +2,29 @@
//https://www.pololu.com/product/2997
//https://www.pololu.com/product/4861
//
-// motor one
-int m1penA = 1;
+// motor one/A front
+int m1enA = 1;
int m1enB = 2;
int m1pwm1 = 3;
int m1pwm2 = 4;
int m1encA = 13;
int m1encB = 14;
-// motor two
+// motor two/B left
int m2enA = 5;
int m2enB = 6;
int m2pwm1 = 7;
int m2pwm2 = 8;
-// motor three
+int m2encA = 15;
+int m2encB = 16;
+// motor three/C right
int m3enA = 9;
int m3enB = 10;
int m3pwm1 = 11;
int m3pwm2 = 12;
+int m3encA = 17;
+int m3encB = 18;
-int sp = 160;
+double sp = 1.000; //speed in 0-1
double dirA = 0.00;
double dirB = 0.00;
@@ -29,6 +33,23 @@ double spdA = 0.00;
double spdB = 0.00;
double spdC = 0.00;
double dirrad = 0.00;
+double pi = 3.1415926535897;
+
+double Arpm = 0.00;
+volatile long Aencount = 0;
+unsigned long ALtime = 0;
+unsigned long ACtime = 0;
+unsigned long APtime = 0;
+double Brpm = 0.00;
+volatile long Bencount = 0;
+unsigned long BLtime = 0;
+unsigned long BCtime = 0;
+unsigned long BPtime = 0;
+double Crpm = 0.00;
+volatile long Cencount = 0;
+unsigned long CLtime = 0;
+unsigned long CCtime = 0;
+unsigned long CPtime = 0;
void setup()
{
@@ -47,146 +68,189 @@ void setup()
pinMode(m3pwm1, OUTPUT);
pinMode(m3pwm2, OUTPUT);
+ //set encoder pins to input
pinMode(m1encA, INPUT);
pinMode(m1encB, INPUT);
+ pinMode(m2encA, INPUT);
+ pinMode(m2encB, INPUT);
+ pinMode(m3encA, INPUT);
+ pinMode(m3encB, INPUT);
+ attachInterrupt(digitalPinToInterrupt(m1encA), AencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m1encB), AencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m2encA), BencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m2encB), BencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m3encA), CencoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(m3encB), CencoderISR, CHANGE);
+}
+
+void AencoderISR() {
+ Aencount++;
+}
+void BencoderISR() {
+ Bencount++;
+}
+void CencoderISR() {
+ Cencount++;
+}
+void Areadenc() {
+ ACtime = micros();
+ APtime = ACtime - ALtime;
+ ALtime = ACtime;
+ Arpm = (Aencount / 211.2) * (60000000.0 / APtime);
+ Serial.print("Motor A RPM: ");
+ Serial.print(Arpm);
+ Serial.print(" Sample period:");
+ Serial.print(APtime);
+ Serial.print("ms, encoder count:");
+ Serial.println(Aencount);
+ Aencount = 0;
+}
+void Breadenc() {
+ BCtime = micros();
+ BPtime = BCtime - BLtime;
+ BLtime = BCtime;
+ Brpm = (Bencount / 211.2) * (60000000.0 / BPtime);
+ Serial.print("Motor B RPM: ");
+ Serial.print(Brpm);
+ Serial.print(" Sample period: ");
+ Serial.print(BPtime);
+ Serial.print(" ms, encoder count: ");
+ Serial.println(Bencount);
+ Bencount = 0;
+}
+void Creadenc() {
+ CCtime = micros();
+ CPtime = CCtime - CLtime;
+ CLtime = CCtime;
+ Crpm = (Cencount / 211.2) * (60000000.0 / CPtime);
+ Serial.print("Motor C RPM: ");
+ Serial.print(Crpm);
+ Serial.print(" Sample period: ");
+ Serial.print(CPtime);
+ Serial.print(" ms, encoder count: ");
+ Serial.println(Cencount);
+ Cencount = 0;
}
-void Amove(int sped){
+void Amove(double sped) {
+ spdA = abs(sped)*255;
digitalWrite(m1enA, HIGH);
- digitalWrite(m1enA, LOW);
- sp = abs(sped);
+ digitalWrite(m1enB, LOW);
if (sped < 0) {
- Serial.print("back");
- analogWrite(m1pwm1, sp);
- digitalWrite(m1pwm2, LOW);
+ Serial.print("Motor A back ");
+ analogWrite(m1pwm1, spdA);
+ analogWrite(m1pwm2, 0);
} else {
- Serial.print("forward");
- digitalWrite(m1pwm1, LOW);
- analogWrite(m1pwm2, sp);
+ Serial.print("Motor A forward ");
+ analogWrite(m1pwm1, 0);
+ analogWrite(m1pwm2, spdA);
}
- Serial.println(sp);
+ Serial.println(spdA);
}
-void Bmove(int sped) {
- Serial.print("moving B ");
+void Bmove(double sped) {
+ spdB = abs(sped)*255;
+ digitalWrite(m2enA, HIGH);
+ digitalWrite(m2enB, LOW);
if (sped < 0) {
- digitalWrite(in3, LOW);
- digitalWrite(in4, HIGH);
- Serial.print("back");
+ Serial.print("Motor B back ");
+ analogWrite(m2pwm1, spdB);
+ analogWrite(m2pwm2, 0);
} else {
- digitalWrite(in3, HIGH);
- digitalWrite(in4, LOW);
- Serial.print("forward");
+ Serial.print("Motor B forward ");
+ analogWrite(m2pwm1, 0);
+ analogWrite(m2pwm2, spdB);
}
- Serial.print("with speed: ");
- int sp = abs(sped);
- analogWrite(enB, sp);
- Serial.println(sp);
+ Serial.println(spdB);
}
-void Cmove(int sped) {
- Serial.print("moving C ");
+void Cmove(double sped) {
+ spdC = abs(sped) * 255;
+ digitalWrite(m3enA, HIGH);
+ digitalWrite(m3enB, LOW);
if (sped < 0) {
- digitalWrite(in5, LOW);
- digitalWrite(in6, HIGH);
- Serial.print("back");
+ Serial.print("Motor C back ");
+ analogWrite(m3pwm1, spdC);
+ analogWrite(m3pwm2, 0);
} else {
- digitalWrite(in5, HIGH);
- digitalWrite(in6, LOW);
- Serial.print("forward");
+ Serial.print("Motor C forward ");
+ analogWrite(m3pwm1, 0);
+ analogWrite(m3pwm2, spdC);
}
- Serial.print("with speed: ");
- int sp = abs(sped);
- analogWrite(enC, sp);
- Serial.println(sp);
-}
-
-void turnoff(){
- Serial.println("stop");
- analogWrite(enA, 0);
- analogWrite(enB, 0);
- analogWrite(enC, 0);
- digitalWrite(in1, LOW);
- digitalWrite(in2, LOW);
- digitalWrite(in3, LOW);
- digitalWrite(in4, LOW);
- digitalWrite(in5, LOW);
- digitalWrite(in6, LOW);
-}
-void forward(int speed){
- Serial.println("forward");
- analogWrite(enA, speed);
- analogWrite(enB, speed);
- analogWrite(enC, speed);
- digitalWrite(in1, LOW);
- digitalWrite(in2, LOW);
- digitalWrite(in3, HIGH);
- digitalWrite(in4, LOW);
- digitalWrite(in5, HIGH);
- digitalWrite(in6, LOW);
-}
-void turn(int speed){
- Serial.println("turn");
- analogWrite(enA, speed);
- analogWrite(enB, speed);
- analogWrite(enC, speed);
- digitalWrite(in1, HIGH);
- digitalWrite(in2, LOW);
- digitalWrite(in3, HIGH);
- digitalWrite(in4, LOW);
- digitalWrite(in5, HIGH);
- digitalWrite(in6, LOW);
+ Serial.println(spdC);
}
void moveohmi(int speed, int direction){
- turnoff();
- double dirrad = (direction*71) / 4068;
- dirA = 1.047198-dirrad;
- dirB = 3.141593-dirrad;
- dirC = 5.235988-dirrad;
- spdA = speed*sin(dirA);
- spdB = speed*sin(dirB);
- spdC = speed*sin(dirC);
- Amove(spdA);
- Bmove(spdB);
- Cmove(spdC);
-}
-void beddermoveohmi(int speed, int direction){
- turnoff();
+ Amove(0);
+ Bmove(0);
+ Cmove(0);
- double dirrad = (direction*71) / 4068;
+ double dirrad = (direction*71.0000) / 4068.0000;
Serial.println(dirrad);
//vector ontb in x en y
- double x = sin(dirrad)*speed;
- double y = cos(dirrad)*speed;
+ double y = sin(dirrad);
+ double x = cos(dirrad);
Serial.println((String)"x:"+x+" y:"+y);
- spdA = -0.5*x - sqrt(3)/2*y;
- spdB = x;
- spdC = -0.5*x + sqrt(3)/2*y;
- Amove(spdA);
- Bmove(spdB);
- Cmove(spdC);
-
- Serial.println(spdA);
- Serial.println(spdB);
- Serial.println(spdC);
+ double spdeA = y*speed;
+ double spdeB = (-0.500*x - sqrt(3.000)/2.000*y)*speed;
+ double spdeC = (-0.500*x + sqrt(3.000)/2.000*y)*speed;
+ Serial.println(spdeA);
+ Amove(spdeA);
+ Bmove(spdeB);
+ Cmove(spdeC);
Serial.println();
Serial.println();
}
-void loop()
-{
+void rotate(int rodeg, int speed){
+//use compas heading
+}
+void demo(){
+
+ Amove(sp);
+ Bmove(sp);
+ Cmove(sp);
+ Serial.println("seperate A B C rotation");
delay(1000);
- turnoff();
- beddermoveohmi(255, 0);
+
+ Amove(0);
+ Bmove(0);
+ Cmove(0);
+ Serial.println("seperate A B C stop");
delay(1000);
- turnoff();
- Amove(255);
+
+ Amove(-sp);
+ Bmove(-sp);
+ Cmove(-sp);
+ Serial.println("seperate A B C -rotation");
delay(1000);
- turnoff();
- Bmove(255);
+
+ moveohmi(0, 0);
+ Serial.println("ohmi stop");
delay(1000);
- turnoff();
- Cmove(255);
+
+ rotate(90, sp);
+ Serial.println("rotation function 90deg");
+ rotate(90, sp);
+ Serial.println("rotation function -90deg");
+ delay(1000);
+
+ moveohmi(sp, 35);
+ Serial.println("ohmi move 35deg");
delay(1000);
- turnoff();
- turn(254);
+
+ moveohmi(sp, 215);
+ Serial.println("ohmi move 215deg");
delay(1000);
+
+ for(int i = 0; i<360; i++){
+ moveohmi(sp, i);
+ Serial.println(i);
+ delay(50);
+ }
+ Serial.println("ohmi move circle");
+ delay(1000);
+
+}
+void loop(){
+ demo();
+ delay(100);
+
}
diff --git a/debug/motordriver/onemotor/onemotor.ino b/debug/motordriver/onemotor/onemotor.ino
new file mode 100644
index 0000000..f3d91e6
--- /dev/null
+++ b/debug/motordriver/onemotor/onemotor.ino
@@ -0,0 +1,88 @@
+#include <Arduino.h>
+
+int enA = 1;
+int enB = 2;
+int pwm1 = 3;
+int pwm2 = 4;
+int encoA = 5;
+int encoB = 6;
+
+int speed = 100; // speed 0-100
+
+double sp = 0.00;
+double rpm = 0.00;
+volatile long encount = 0;
+unsigned long Ltime = 0;
+unsigned long Ctime = 0;
+unsigned long Ptime = 0;
+
+void setup() {
+ pinMode(enA, OUTPUT);
+ pinMode(enB, OUTPUT);
+ pinMode(pwm1, OUTPUT);
+ pinMode(pwm2, OUTPUT);
+
+ pinMode(encoA, INPUT_PULLUP);
+ pinMode(encoB, INPUT_PULLUP);
+
+ attachInterrupt(digitalPinToInterrupt(encoA), encoderISR, CHANGE);
+ attachInterrupt(digitalPinToInterrupt(encoB), encoderISR, CHANGE);
+
+ Serial.begin(9600);
+}
+
+void encoderISR() {
+ encount++;
+}
+
+void readenc() {
+ Ctime = micros();
+ Ptime = Ctime - Ltime;
+ Ltime = Ctime;
+ rpm = (encount / 211.2) * (60000000.0 / Ptime);
+ Serial.print("RPM: ");
+ Serial.print(rpm);
+ Serial.print(" Sample period:");
+ Serial.print(Ptime);
+ Serial.print("ms, encoder count:");
+ Serial.println(encount);
+
+ encount = 0;
+}
+void resetenc(){
+ Ctime = micros();
+ Ltime = Ctime;
+ encount = 0;
+}
+
+void move(int sped) {
+ sp = abs(sped)*2.55;
+ digitalWrite(enA, HIGH);
+ digitalWrite(enB, LOW);
+ if (sped < 0) {
+ Serial.print("back ");
+ analogWrite(pwm1, sp);
+ analogWrite(pwm2, 0);
+ } else {
+ Serial.print("forward ");
+ analogWrite(pwm1, 0);
+ analogWrite(pwm2, sp);
+ }
+ Serial.println(sp);
+}
+
+void loop() {
+ move(speed);
+ delay(1000);
+ readenc();
+
+
+ move(0);
+ resetenc();
+ delay(1000);
+ readenc();
+
+ move(-speed);
+ delay(1000);
+ readenc();
+}
generated by cgit v1.2.3 (git 2.46.0) at 2026-09-30 03:36:19 +0000