1 files changed, 112 insertions, 24 deletions

diff --git a/code/debug/motors/base_drive/src/main.cpp b/code/debug/motors/base_drive/src/main.cpp
index 907736b..1130afe 100644
--- a/code/debug/motors/base_drive/src/main.cpp
+++ b/code/debug/motors/base_drive/src/main.cpp
@@ -1,6 +1,13 @@
-//base code for six direction driving
 
+//motor: https://www.pololu.com/product/4861
+//motor driver: https://www.pololu.com/product/2997
 
+//Create the timers
+IntervalTimer pwm_timer;
+IntervalTimer enc_timer;
+
+//define the pins
+//If the motors are spinning incorectly, change pins not the drive function.
 int led = 13;
 
 int m1enA = 6;
@@ -23,28 +30,120 @@ int m3pwm1 = 24;
 int m3pwm2 = 25;
 int m3encoA = 28;
 int m3encoB = 23;
-  
+
+//PWM variables
+int pwmacc = 100; //how accurate the PWM is, means you can enter value from 0 to the value of pwmacc
+int PWMval1 = 0;  //value of the PWM1, means you can enter value from 0 to the value of pwmacc
+
+//encoder variables
+const int refreshrate = 50; //refresh rate of the encoder
+volatile long oneENCcount = 0; //the amount of pulses from motor one encoder
+
+double ONErpm = 0.00; //the amount of revolutions motor one makes per minute
+
+
+
 void setup() {
   pinMode(m1enA, OUTPUT);
   pinMode(m1enB, OUTPUT);
   pinMode(m1pwm1, OUTPUT);
   pinMode(m1pwm2, OUTPUT);
-  
-  pinMode(m2enA, OUTPUT);
-  pinMode(m2enB, OUTPUT);
-  pinMode(m2pwm1, OUTPUT);
-  pinMode(m2pwm2, OUTPUT);
-  
-  pinMode(m3enA, OUTPUT);
-  pinMode(m3enB, OUTPUT);
-  pinMode(m3pwm1, OUTPUT);
-  pinMode(m3pwm2, OUTPUT);
+
+  pwm_timer.begin(updatePWM, refreshrate); //speed at wich PWM is updated PWM
+  enc_timer.begin(enc_refr, 50);  //refresh rate of the encoder
+
+  attachInterrupt(digitalPinToInterrupt(m1encoA), AoneIRS, CHANGE); //calls void AoneIRS if there is a change of the stare of m1encoA
+  attachInterrupt(digitalPinToInterrupt(m1encoB), BoneIRS, CHANGE); //ditto
+
   Serial.begin(9600);
   Serial.print("start");
-  
+  SetTunings(1, 1, 1);
+
 }
 
+void updatePWM() {
+  int counter1 = 0;
+  //PWM one
+  //if the entered valvue is less the zero set it to zero, because lower that zero is not posible
+  if (PWMval1 < 0) {
+    PWMval1 = 0;
+  }
+  //if the enterd value is higher than the maximum posible value, set it to max.
+  if (PWMval1 > pwmacc) {
+    PWMval1 = pwmacc;
+  }
+  //check if the counter has passed the PWM value, and turn of the motor based on that.
+  if (counter1 < PWMval1) {
+    digitalWrite(m1enA, HIGH);
+  } else {
+    digitalWrite(m1enA, LOW);
+  }
+  counter1++;
 
+  if (counter1 >= pwmacc) {
+    counter1 = 0;
+  }
+}
+
+void AoneIRS(){
+  oneENCcount++; //adds one to the count
+}
+void BoneIRS(){
+  oneENCcount++; //dieselbe
+}
+
+void enc_refr(){
+  //calculate RPM. It gets updated 48 times per rotation.
+  ONErpm = (oneENCcount/48.00) * (60000000.00 / refreshrate);
+  //reset the counting to zero for next cycle.
+  oneENCcount = 0;
+}
+void PID(int speed){
+  Serial.println(ONErpm);
+  PWMval1 = Compute(); //enter output from PID algorith here
+  digitalWrite(m1enB, LOW);
+  digitalWrite(m1pwm1, HIGH);
+  digitalWrite(m1pwm2, LOW);
+}
+
+/*working variables*/
+unsigned long lastTime;
+double Input, Output, Setpoint;
+double errSum, lastErr;
+double kp, ki, kd;
+int Compute()
+{
+  Input = ONErpm;
+  /*How long since we last calculated*/
+  unsigned long now = millis();
+  double timeChange = (double)(now - lastTime);
+ 
+  /*Compute all the working error variables*/
+  double error = Setpoint - Input;
+  errSum += (error * timeChange);
+  double dErr = (error - lastErr) / timeChange;
+ 
+/*Compute PID Output*/
+ 
+/*Remember some variables for next time*/
+  lastErr = error;
+  lastTime = now;
+  return kp * error + ki * errSum + kd * dErr;
+}
+ 
+void SetTunings(double Kp, double Ki, double Kd)
+{
+kp = Kp;
+ki = Ki;
+kd = Kd;
+}
+
+//the loop routine runs over and over again forever:
+void loop() {
+  drive(50, 0); //speed value between 0-100
+  //Serial.println("Motor one is at: " + ONErpm + " RPM.");
+  delay(50);
+}
 
 void drive(int speed, int direction){
  //
@@ -132,14 +231,3 @@ void drive(int speed, int direction){
     Serial.println("how?");
   }
 }
-
-// the loop routine runs over and over again forever:
-void loop() {
-  drive(255, 0);
-  Serial.println("ffor");
-  delay(1500);
-  drive(255, 180);
-  Serial.println("back");
-  delay(1500);
-
-}