poster

1 files changed, 192 insertions, 0 deletions

diff --git a/motordriver/nomultiplexer/nomultiplexer.ino b/motordriver/nomultiplexer/nomultiplexer.ino
new file mode 100644
index 0000000..d2800a8
--- /dev/null
+++ b/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);
+}