Revision 0:5ffcaef3e603, committed 2018-04-18

Comitter:

Weranest

Date:

Wed Apr 18 16:53:05 2018 +0000

Commit message:

Control added

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.cpp	Wed Apr 18 16:53:05 2018 +0000
@@ -0,0 +1,97 @@
+#include "mbed.h"
+#include "C12832.h"
+
+C12832 lcd(D11, D13, D12, D7, D10);
+
+AnalogIn pot1 (A0);
+AnalogIn pot2 (A1);
+//sensors pin
+AnalogIn SRI (PC_4);
+AnalogIn SLI (PC_2);
+AnalogIn SRO (PB_1);
+AnalogIn SLO (PC_5);
+//AnalogIn SC (PB_0);
+AnalogIn SM (PC_3);
+//motor pin
+PwmOut motorRight(PC_6);
+PwmOut motorLeft(PC_8);
+DigitalOut motorDirRight(PH_1);
+DigitalOut motorDirLeft(PH_0);
+DigitalOut motorModeRight(PC_15);
+DigitalOut motorModeLeft(PC_14);
+DigitalOut driveBoard (PB_2);
+
+void turnArround();
+
+void motorSetup(){
+    motorRight.period_ms(1.0f);
+    motorLeft.period_ms(1.0f);
+    motorDirLeft.write(0);
+    motorDirRight.write(0);
+    motorModeLeft.write(0); //1 =bipolar, 0 = unipolar
+    motorModeRight.write(0); 
+    driveBoard.write(1); //enables the buggy to drive
+}
+
+int main()
+{
+    float SRIV, SLIV, SROV, SLOV, SMV;
+    float kp=0.07125,ki=0.0,kd=0.02875;
+    float error1, error2, error3,error,perror=0,errort=0, pidvalue;
+    float pr, pl;
+    
+    motorSetup();
+    lcd.cls();
+    //motorRight.write(1.0f);
+    //motorLeft.write(1.0f);
+    
+    
+    while(1){
+        /*lcd.locate(0,3);
+        lcd.printf("LeftO= %.2fV/ RightO= %.2fV", (float)SLO*3.30, (float)SRO*3.30);
+        lcd.locate(0,13);
+        lcd.printf("LeftI= %.2fV/ RightI= %.2fV", (float)SLI*3.30, (float)SRI*3.30);
+        lcd.locate(0,23);
+        lcd.printf("Magn= %.2fV",  (float)SM *3.30);*/
+        
+        SLOV=(float)SLO*3.30;
+        SLIV=(float)SLI*3.30;
+        SRIV=(float)SRI*3.30;
+        SROV=(float)SRO*3.30;
+        SMV=(float)SM *3.30;
+        
+        error1=SLOV-SROV;
+        error2=SLIV-SRIV;
+        error=error1+error2;
+        //errort+=error;
+        pidvalue=kp*error+kd*(error-perror)+ki*errort;
+        
+        if ((SMV>2.7f)||(SMV<0.6f)) turnArround();
+        motorRight.write(0.72f-pidvalue);
+        motorLeft.write(0.7411f+pidvalue);
+        
+        pr=motorRight.read();
+        pl=motorLeft.read();
+        
+        lcd.locate(0,3);
+        lcd.printf("pr:%.4fV pl:%.4fV", pr, pl);
+        lcd.locate(0,13);
+        lcd.printf("e1= %.4fV/ e2= %.4fV", error1, error2);
+        perror=error;
+        pidvalue=0;
+        
+    }
+    
+}
+
+void turnArround(){ //turns us around
+     motorRight.write(1.0f);
+     motorLeft.write(1.0f);
+     wait(3);
+     motorDirLeft.write(1);
+     motorDirRight.write(0);
+     motorRight.write(0.8f);
+     motorLeft.write(0.8f);
+     wait(3);
+     motorDirLeft.write(0);
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/driveBoardBase.cpp	Wed Apr 18 16:53:05 2018 +0000
@@ -0,0 +1,8 @@
+#include "mbed.h"
+#include "pwm.cpp"
+
+int main(){
+    motorSetup();
+    return 0;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderBase.cpp	Wed Apr 18 16:53:05 2018 +0000
@@ -0,0 +1,47 @@
+/*#include "mbed.h"
+#include "QEI.h"
+#include "pins.cpp"
+#define ENCODER_PULSE 256 //per manual
+#define GEAR_RATIO 
+#define SAMPLE_TIME 1.0f //user defined
+#define WHEEL_DISTANCE //need to measure that
+
+//these lines setup the encoders for both wheels
+QEI encoderRight(channelARight, channelBRight, channelIRight, ENCODER_PULSE);
+QEI encoderLeft(channelALeft, channelBLeft, channelILeft, ENCODER_PULSE);
+
+//main wheel class, the subclasses should follow later
+class Wheel{
+    int currentPulses, previousPulses;
+    public:
+        float wheelVelocity(float, int){
+        }
+    private:
+        float sampleTime= SAMPLE_TIME;
+        int gearRatio = GEAR_RATIO;
+        float encoderVelocity(){
+        return (currentEncoderTicks-previousEncoderTicks)/sampleTime;
+        }
+
+} wheelRight, wheelLeft;
+
+class WheelLeft
+
+//call this for speed, linear and angular
+class Buggy{
+    public:
+        float speedLinear(float wheelVelocityRight, float wheelVelocityLeft){
+            return (wheelVelocityRight+wheelVelocityLeft)/2;
+            }
+        float speedAngular(float wheelVelocityRight, float wheelVelocityLeft){
+            return (wheelVelocityRight-wheelVelocityLeft)/WHEEL_DISTANCE;
+            }
+    }
+int encoderTicks(){
+        previousEncoderTicks=currentEncoderTicks;
+        currentEncoderTicks=
+        }
+
+
+*/
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 18 16:53:05 2018 +0000
@@ -0,0 +1,9 @@
+#include "mbed.h"
+#include "pwm.cpp"
+
+int main(){
+    motorSetup();
+    buggyGoF();
+    while(1);
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 18 16:53:05 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/7130f322cb7e
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pins.cpp	Wed Apr 18 16:53:05 2018 +0000
@@ -0,0 +1,24 @@
+//This file is strictly to define pins for buggy operations. Thats it. But thought it would be easier to overseer them like this
+#include "mbed.h"
+//Encoder pins (created as a QEI object, so just defined naming for pins)
+//=======================
+#define channelARight PA_1
+#define channelBRight 
+#define channelIRight 
+#define channelALeft 
+#define channelBLeft 
+#define channelILeft 
+//Motor control pins
+//=======================
+PwmOut motorRight(PB_6);
+PwmOut motorLeft(PB_7);
+DigitalOut motorDirRight(PB_4);
+DigitalOut motorDirLeft(PB_5);
+DigitalOut motorModeRight(PB_2);
+DigitalOut motorModeLeft(PB_3);
+DigitalOut driveBoard (PB_1);
+//Line sensor pins
+//=======================
+//Magnet sensor pins
+//=======================
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pwm.cpp	Wed Apr 18 16:53:05 2018 +0000
@@ -0,0 +1,40 @@
+//this defines motor setup and movement
+#include "mbed.h"
+#include "pins.cpp"
+//will have to determine real pulsewidths by testing with encoder
+//constants to be utilized in the buggy, can be changed as required
+#define NORMAL_PWM 0.2f
+#define FAST_PWM 0.7f
+#define SLOW_PWM 0.3f
+#define RIGHT_MOTOR_CONST 1
+#define LEFT_MOTOR_CONST 1
+#define MOTOR_PERIOD 1.0f
+//currently the setup uses unipolar mode, hence the need for direction
+
+
+void motorSetup(){
+    motorRight.period_ms(MOTOR_PERIOD);
+    motorLeft.period_ms(MOTOR_PERIOD);
+    motorDirLeft.write(1);
+    motorDirRight.write(1);
+    motorModeLeft.write(1);
+    motorModeRight.write(1);
+    driveBoard.write(1);
+}
+void buggyGoF(){ //drives forward
+    motorRight.write(NORMAL_PWM);
+    motorLeft.write(NORMAL_PWM);
+}
+
+void buggyGoLeft(){// drives left
+    motorRight.write(FAST_PWM);
+    motorLeft.write(SLOW_PWM);
+}
+void buggyGoRight(){ //drives fast
+    motorRight.write(SLOW_PWM);
+    motorLeft.write(FAST_PWM);
+}
+void buggyStop(){// no power
+    motorRight.write(0.0f);
+    motorLeft.write(0.0f);
+}