summaryrefslogtreecommitdiff
path: root/debug/motordriver
diff options
context:
space:
mode:
Diffstat (limited to 'debug/motordriver')
-rw-r--r--debug/motordriver/motordriver.ino258
-rw-r--r--debug/motordriver/nomultiplexer/nomultiplexer.ino192
2 files changed, 450 insertions, 0 deletions
diff --git a/debug/motordriver/motordriver.ino b/debug/motordriver/motordriver.ino
new file mode 100644
index 0000000..0fd30e7
--- /dev/null
+++ b/debug/motordriver/motordriver.ino
@@ -0,0 +1,258 @@
+//moveohmi = https://robotics.stackexchange.com/questions/7829/how-to-program-a-three-wheel-omni
+
+
+#include <Adafruit_MCP23X17.h>
+
+// motor one
+int enA = 33;
+int in1 = 2;
+int in2 = 3;
+// motor two
+int enB = 36;
+int in3 = 4;
+int in4 = 5;
+// motor three
+int enC = 37;
+int in5 = 7;
+int in6 = 6;
+
+int sp = 160;
+
+double dirA = 0.00;
+double dirB = 0.00;
+double dirC = 0.00;
+double spdA = 0.00;
+double spdB = 0.00;
+double spdC = 0.00;
+double dirrad = 0.00;
+
+void setup()
+{
+ // set all the motor control pins to outputs
+ pinMode(enA, OUTPUT);
+ pinMode(enB, OUTPUT);
+ pinMode(enC, OUTPUT);
+
+ pinMode(in1, OUTPUT);
+ pinMode(in2, OUTPUT);
+ pinMode(in3, OUTPUT);
+ pinMode(in4, OUTPUT);
+ pinMode(in5, OUTPUT);
+ pinMode(in6, OUTPUT);
+}
+void demoOne()
+{
+// this function will run the motors in both directions at a fixed speed
+ // turn on motor A
+ digitalWrite(in1, HIGH);
+ digitalWrite(in2, LOW);
+ // set speed to 200 out of possible range 0~255
+ analogWrite(enA, sp);
+ // turn on motor B
+ digitalWrite(in3, HIGH);
+ digitalWrite(in4, LOW);
+ // set speed to 200 out of possible range 0~255
+ analogWrite(enB, sp);
+ // turn on motor c
+ digitalWrite(in5, HIGH);
+ digitalWrite(in6, LOW);
+ // set speed to 200 out of possible range 0~255
+ analogWrite(enC, sp);
+ delay(2000);
+ // now change motor directions
+ digitalWrite(in1, LOW);
+ digitalWrite(in2, HIGH);
+ digitalWrite(in3, LOW);
+ digitalWrite(in4, HIGH);
+ digitalWrite(in5, LOW);
+ digitalWrite(in6, HIGH);
+ delay(2000);
+ // now turn off motors
+ digitalWrite(in1, LOW);
+ digitalWrite(in2, LOW);
+ digitalWrite(in3, LOW);
+ digitalWrite(in4, LOW);
+}
+void demoTwo()
+{
+ // this function will run the motors across the range of possible speeds
+ // note that maximum speed is determined by the motor itself and theoperating voltage
+ // the PWM values sent by analogWrite() are fractions of the maximumspeed possible
+ // by your hardware
+ // turn on motors
+ digitalWrite(in1, LOW);
+ digitalWrite(in2, HIGH);
+ digitalWrite(in3, LOW);
+ digitalWrite(in4, HIGH);
+ digitalWrite(in5, LOW);
+ digitalWrite(in6, HIGH);
+ // accelerate from zero to maximum speed
+ for (int i = 0; i < 256; i++)
+ {
+ analogWrite(enA, i);
+ analogWrite(enB, i);
+ analogWrite(enC, i);
+ delay(20);
+ }
+ // decelerate from maximum speed to zero
+ for (int i = 255; i >= 0; --i)
+ {
+ analogWrite(enA, i);
+ analogWrite(enB, i);
+ analogWrite(enC, i);
+ delay(20);
+ }
+ // now turn off motors
+ digitalWrite(in1, LOW);
+ digitalWrite(in2, LOW);
+ digitalWrite(in3, LOW);
+ digitalWrite(in4, LOW);
+ digitalWrite(in5, LOW);
+ digitalWrite(in6, LOW);
+}
+
+void Amove(int sped){
+ Serial.print("moving A ");
+ if (sped < 0) {
+ digitalWrite(in1, LOW);
+ digitalWrite(in2, HIGH);
+ Serial.print("back");
+ }
+ else{
+ digitalWrite(in1, HIGH);
+ digitalWrite(in2, LOW);
+ Serial.print("forward");
+ }
+ Serial.print("with speed: ");
+ sp = abs(sped);
+ analogWrite(enA, sp);
+ Serial.println(sp);
+}
+void Bmove(int sped) {
+ Serial.print("moving B ");
+ if (sped < 0) {
+ digitalWrite(in3, LOW);
+ digitalWrite(in4, HIGH);
+ Serial.print("back");
+ } else {
+ digitalWrite(in3, HIGH);
+ digitalWrite(in4, LOW);
+ Serial.print("forward");
+ }
+ Serial.print("with speed: ");
+ int sp = abs(sped);
+ analogWrite(enB, sp);
+ Serial.println(sp);
+}
+void Cmove(int sped) {
+ Serial.print("moving C ");
+ if (sped < 0) {
+ digitalWrite(in5, LOW);
+ digitalWrite(in6, HIGH);
+ Serial.print("back");
+ } else {
+ digitalWrite(in5, HIGH);
+ digitalWrite(in6, LOW);
+ Serial.print("forward");
+ }
+ 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);
+}
+
+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();
+
+ double dirrad = (direction*71) / 4068;
+ Serial.println(dirrad);
+ //vector ontb in x en y
+ double x = sin(dirrad)*speed;
+ double y = cos(dirrad)*speed;
+ 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);
+ Serial.println();
+ Serial.println();
+}
+void loop()
+{
+ //demoOne();
+ //delay(1000);
+ //demoTwo();
+ delay(1000);
+ turnoff();
+ beddermoveohmi(255, 0);
+ delay(1000);
+ turnoff();
+ Amove(255);
+ delay(1000);
+ turnoff();
+ Bmove(255);
+ delay(1000);
+ turnoff();
+ Cmove(255);
+ delay(1000);
+ turnoff();
+ turn(254);
+ delay(1000);
+}
diff --git a/debug/motordriver/nomultiplexer/nomultiplexer.ino b/debug/motordriver/nomultiplexer/nomultiplexer.ino
new file mode 100644
index 0000000..d2800a8
--- /dev/null
+++ b/debug/motordriver/nomultiplexer/nomultiplexer.ino
@@ -0,0 +1,192 @@
+//moveohmi = https://robotics.stackexchange.com/questions/7829/how-to-program-a-three-wheel-omni
+//https://www.pololu.com/product/2997
+//https://www.pololu.com/product/4861
+//
+// motor one
+int m1penA = 1;
+int m1enB = 2;
+int m1pwm1 = 3;
+int m1pwm2 = 4;
+int m1encA = 13;
+int m1encB = 14;
+// motor two
+int m2enA = 5;
+int m2enB = 6;
+int m2pwm1 = 7;
+int m2pwm2 = 8;
+// motor three
+int m3enA = 9;
+int m3enB = 10;
+int m3pwm1 = 11;
+int m3pwm2 = 12;
+
+int sp = 160;
+
+double dirA = 0.00;
+double dirB = 0.00;
+double dirC = 0.00;
+double spdA = 0.00;
+double spdB = 0.00;
+double spdC = 0.00;
+double dirrad = 0.00;
+
+void setup()
+{
+ // set all the motor control pins to outputs
+ pinMode(m1enA, OUTPUT);
+ pinMode(m1enB, OUTPUT);
+ pinMode(m2enA, OUTPUT);
+ pinMode(m2enB, OUTPUT);
+ pinMode(m3enA, OUTPUT);
+ pinMode(m3enB, OUTPUT);
+
+ pinMode(m1pwm1, OUTPUT);
+ pinMode(m1pwm2, OUTPUT);
+ pinMode(m2pwm1, OUTPUT);
+ pinMode(m2pwm2, OUTPUT);
+ pinMode(m3pwm1, OUTPUT);
+ pinMode(m3pwm2, OUTPUT);
+
+ pinMode(m1encA, INPUT);
+ pinMode(m1encB, INPUT);
+}
+
+void Amove(int sped){
+ digitalWrite(m1enA, HIGH);
+ digitalWrite(m1enA, LOW);
+ sp = abs(sped);
+ if (sped < 0) {
+ Serial.print("back");
+ analogWrite(m1pwm1, sp);
+ digitalWrite(m1pwm2, LOW);
+ } else {
+ Serial.print("forward");
+ digitalWrite(m1pwm1, LOW);
+ analogWrite(m1pwm2, sp);
+ }
+ Serial.println(sp);
+}
+void Bmove(int sped) {
+ Serial.print("moving B ");
+ if (sped < 0) {
+ digitalWrite(in3, LOW);
+ digitalWrite(in4, HIGH);
+ Serial.print("back");
+ } else {
+ digitalWrite(in3, HIGH);
+ digitalWrite(in4, LOW);
+ Serial.print("forward");
+ }
+ Serial.print("with speed: ");
+ int sp = abs(sped);
+ analogWrite(enB, sp);
+ Serial.println(sp);
+}
+void Cmove(int sped) {
+ Serial.print("moving C ");
+ if (sped < 0) {
+ digitalWrite(in5, LOW);
+ digitalWrite(in6, HIGH);
+ Serial.print("back");
+ } else {
+ digitalWrite(in5, HIGH);
+ digitalWrite(in6, LOW);
+ Serial.print("forward");
+ }
+ 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);
+}
+
+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();
+
+ double dirrad = (direction*71) / 4068;
+ Serial.println(dirrad);
+ //vector ontb in x en y
+ double x = sin(dirrad)*speed;
+ double y = cos(dirrad)*speed;
+ 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);
+ Serial.println();
+ Serial.println();
+}
+void loop()
+{
+ delay(1000);
+ turnoff();
+ beddermoveohmi(255, 0);
+ delay(1000);
+ turnoff();
+ Amove(255);
+ delay(1000);
+ turnoff();
+ Bmove(255);
+ delay(1000);
+ turnoff();
+ Cmove(255);
+ delay(1000);
+ turnoff();
+ turn(254);
+ delay(1000);
+}
generated by cgit v1.2.3 (git 2.46.0) at 2026-09-30 01:18:46 +0000