Diff: main.cpp

Revision:

0:d33abe99b8aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 18 20:13:02 2020 +0000
@@ -0,0 +1,131 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "C12832.h"
+
+
+C12832 lcd(D11, D13, D12, D7, D10);
+        
+class Motor {
+private:
+    PwmOut pwm;
+    DigitalOut direction, pole;
+    float pwmWidth, offset;
+    int high;
+public:
+    Motor(PinName pwmPin, PinName directionPin, PinName polePin, int ofst, int h): pwm(pwmPin), direction(directionPin), pole(polePin), offset(ofst), high(h){
+        pwm.period_us(5);
+        pwmWidth = 0.5+(offset/100.0);
+        pole = 0;
+    }
+    void PWM(double transC, double angC, int drctn){  // PWM(speed error, angle error, direction)
+     
+        if (transC == 0 || angC == 0){
+            pwm.write(1-high);
+        }
+        else{
+             pwmWidth *= transC*angC;
+            if (pwmWidth >= 1){
+                pwmWidth = 1;
+            }
+            if (high == 1){
+                pwm.write(pwmWidth);
+            }
+            else{
+                pwm.write(1-pwmWidth);
+            }
+            direction = drctn;
+        }
+    }
+};
+
+class Encoder{
+private:
+    int pulses, currentSpeed, dispx, dispy;
+    Ticker tkr, disptkr;
+    QEI encoder;
+public:
+    Encoder(PinName CHA, PinName CHB, int x, int y):encoder(CHA, CHB, NC, 624), dispx(x), dispy(y){    
+        tkr.attach(callback(this, &Encoder::findPulses), 0.01);
+        disptkr.attach(callback(this, &Encoder::display), 0.01);
+    }
+    void findPulses(){
+        pulses = encoder.getPulses();
+        encoder.reset(); //set back to 0 wait 0.1 seconds and take another reading
+        currentSpeed = pulses*2; //set this number to circumpherance of the wheel
+        tkr.attach(callback(this, &Encoder::findPulses), 0.1); //frequency of encoder readings. The longer it waits, the more accuate it is ut i nees to maintain a real time response
+    }
+    double findSpeed(){
+        return currentSpeed; 
+    }
+    void display(){ 
+        lcd.locate(dispx ,dispy);
+        lcd.printf("%d ", currentSpeed);
+        disptkr.attach(callback(this, &Encoder::display), 0.1);
+    }
+};
+
+
+
+
+
+
+
+
+//define pins
+Encoder lEncoder(PB_13, PB_14, 10, 10); //chA, chB
+Encoder rEncoder(PB_15, PB_1, 60, 10);
+Motor rMotor(PA_15, PC_13, PC_14, 0, 1); //pwm, direction, pole, ofset, logic sensitivity
+Motor lMotor(PB_7, PC_15, PH_0, 5, 0);  //#######################################
+DigitalOut enable(PA_14);
+
+//define global variables
+
+    
+    
+    
+    
+    
+    
+    
+int main() {
+    
+    double driveTime, turnTime;
+    driveTime = 1;                      //#######################################
+    turnTime = 0.8;                      //#######################################
+    enable = 1;
+    
+    for(int i = 0; i < 3; i++){
+        lMotor.PWM(1, 1, 1);
+        rMotor.PWM(1, 1, 1);
+        wait(driveTime);
+        
+        lMotor.PWM(1, 1, 1);
+        rMotor.PWM(1, 0, 1);
+        wait(turnTime);
+    } 
+    
+    lMotor.PWM(1, 1, 1);
+    rMotor.PWM(1, 1, 1);
+    wait(driveTime);
+    
+    lMotor.PWM(1, 1, 1);
+    rMotor.PWM(1, 0, 1);
+    wait(2*turnTime);
+    
+    for(int i = 0; i < 3; i++){
+        lMotor.PWM(1, 1, 1);
+        rMotor.PWM(1, 1, 1);
+        wait(driveTime);
+        
+        lMotor.PWM(1, 0, 1);
+        rMotor.PWM(1, 1, 1);
+        wait(turnTime);
+    } 
+    
+    lMotor.PWM(1, 1, 1);
+    rMotor.PWM(1, 1, 1);
+    wait(driveTime);
+    
+    lMotor.PWM(0, 1, 1);
+    rMotor.PWM(0, 1, 1);
+}
\ No newline at end of file