1 files changed, 258 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);
+}