Diff: main.cpp

Revision:

1:c324a2849500

Parent:

0:0a6756c7e3ed

Child:

2:c857935f928e

--- a/main.cpp	Fri Sep 10 03:52:04 2021 +0000
+++ b/main.cpp	Sun Oct 24 18:15:30 2021 +0000
@@ -1,61 +1,293 @@
 #include "mbed.h"
+#include "driving.h"
+#include "MMA8451Q.h"
 #include <iostream>
 #define BLUETOOTHBAUDRATE 115200 //Communication rate of the micro-controller
  //to the Bluetooth module
 #define SERIALTXPORT PTE0 //Tx Pin (Sends information to serial device)
 #define SERIALRXPORT PTE1 //Rx Pin (Receives information from serial
- 
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
 Serial pc(USBTX, USBRX); // tx, rx
 Serial bt(SERIALTXPORT, SERIALRXPORT); //(TX, RX) Serial declaration for new serial
 
-void btRecevie()
-{
-string input = bt.getc();
+PinName const SDA = PTE25;
+PinName const SCL = PTE24;
+
+MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+Ticker motorLoop;
+Timer t;
+
+PwmOut motorLeft(PTB1); 
+PwmOut motorRight(PTB2);
+AnalogIn pot1(PTB0);
+AnalogIn speedSensorLeft(PTB3);
+AnalogIn speedSensorRight(PTC2);
+DigitalOut ledRed(LED1);
+AnalogIn battInput(PTC1);
+DigitalOut brakeLeft(PTA12);
+DigitalOut brakeRight(PTD4);
+
+DigitalIn enableBrakeLeft(PTA4);
+DigitalIn enableBrakeRight(PTA5);
+
+float pot1Voltage;
+float dutyCycleLeft;
+float tempDutyCycleLeft = 0;
+float tempDutyCycleRight = 0;
+float dutyCycleRight;
+float speedLeftVolt;
+float speedRightVolt;
+float batteryVoltage;
+float avgCellVoltage;
+
+float errorAreaLeft = 0;
+float errorAreaRight = 0;
+float errorAreaLeftPrev = 0;
+float errorAreaRightPrev = 0;
+float errorLeft = 0;
+float errorRight = 0;
+float setpointLeft = 0.0; //target speed, 0.0 to 1.0
+float setpointRight = 0.0;
+float controllerOutputLeft = 0;
+float controllerOutputRight = 0;
+float x;
+
+char newInputChar;
+int newInputInt;
+bool clampLeft = false;
+bool clampRight = false;
+
+bool brakeLeftBool = false;
+bool brakeRightBool = false;
+
+volatile bool newData = false;
+ 
+void btReceive() { //comment this out if it's fucked
 
+ static char buffer[6];
+ static int serialCount = 0;
+ 
+{
+        char byteIn = bt.getc();
+      //  bt.printf("Got %c",byteIn);
+        if (byteIn == 'n') {
+            buffer[serialCount] = 0;
+            //bt.printf("Got endl %c",byteIn);
+            int speed;
+            char type;
+            if (sscanf(buffer,"%c%i",&type,&speed) == 2) {
+                newInputChar = type;
+               // bt.printf("char: %c", type);
+                newInputInt = speed;
+               // bt.printf("speed: %i", speed);
+                newData = true;
+                }
+            serialCount = 0;
+            } else {
+                buffer[serialCount] = byteIn;
+                if (serialCount < 6)
+                serialCount++;
+                }
+        }
+    }
     
+void PI() { //motor PI interrupt
+
+//--- Calculate Error ---
+errorLeft = setpointLeft - (speedLeftVolt / 3.3f); //error and setpoint is defined as a percentage, from 0 to 1
+errorRight = setpointRight - (speedRightVolt / 3.3f);
+
+//--- Steady State Error Tolerace ---
+if (abs(errorLeft) < 0.01){
+    errorLeft = 0.0;
+    }
+if (abs(errorRight) < 0.01){
+    errorRight = 0.0;
+    }
+//--- Calculate integral error ---
+if (clampLeft == false){
+errorAreaLeft = TI*errorLeft + errorAreaLeftPrev;
 }
-    
+
+if (clampRight == false){ 
+errorAreaRight = TI*errorRight + errorAreaRightPrev;
+}
+
+errorAreaLeftPrev = errorAreaLeft;
+errorAreaRightPrev = errorAreaRight;
+
+/*
+if (errorAreaLeft > 0.1){
+   errorAreaLeft = 0.0;
+   }
+   p
+if (errorAreaRight > 0.1){
+errorAreaRight = 0.0;
+}         
+*/       
+   
+//--- Calculate total error ---  
+controllerOutputLeft = KP_LEFT*errorLeft + KI_LEFT*errorAreaLeft;
+controllerOutputRight = KP_RIGHT*errorRight + KI_RIGHT*errorAreaRight;
+
+tempDutyCycleLeft = fullBatScalar * controllerOutputLeft;
+tempDutyCycleRight = fullBatScalar * controllerOutputRight;
 
 
-AnalogIn pot1(PTB0);
-PwmOut motorRight(PTB2);
-PwmOut motorLeft(PTB1); 
-DigitalOut ledRed(LED1);
+//--- Motor over-voltage safety ---
+if (tempDutyCycleLeft > fullBatScalar){ //safety mechanism in case feedback breaks for any reason - 
+        dutyCycleLeft = fullBatScalar; //will stop output from exceeding max duty cycle and damaging motor
+    } else {                                            
+        dutyCycleLeft = tempDutyCycleLeft;
+}
+
+if (tempDutyCycleRight > fullBatScalar){
+        dutyCycleRight = fullBatScalar;
+    } else {
+        dutyCycleRight = tempDutyCycleRight;
+    }
+    
+
+//--- integral anti-windup ---
+if (controllerOutputLeft > 1.0){
+    if (errorAreaLeft > 0.0){
+        clampLeft = true;
+        }
+    if (errorAreaLeft < 0.0){
+        clampLeft = false;
+        }
+    } else {
+        clampLeft = false;
+}
+
+if (controllerOutputRight > 1.0){
+    if (errorAreaRight > 0.0){
+        clampRight = true;
+        }
+    if (errorAreaRight < 0.0){
+        clampRight = false;
+        }
+    } else {
+        clampRight = false;
+}
+
+//--- fucked up manual braking stuff ---
+if (setpointLeft == 0.0 || brakeLeftBool == true)
+{
+    errorAreaLeft = 0.0;
+    brakeLeft = 1;
+} else {
+    brakeLeft = 0;
+}
+
+if (setpointRight == 0.0 || brakeRightBool == true)
+{
+    errorAreaRight = 0.0;
+    brakeRight = 1;
+} else {
+    brakeRight = 0;
+}
+
+//--- set motors to calculated output ---    
+motorLeft.write(dutyCycleLeft);
+motorRight.write(dutyCycleRight);
 
 
-float freq = 20000;
-float fullBatScalar = 0.5873; //batt at 12.6v 
-float pot1Voltage;
-float dutyCycleLeft;
-float dutyCycleRight;
+//--- motor braking ---
+/*
+if (controllerOutputLeft < 0.0){ 
+brakeLeft = 1;
+    } else {
+        brakeLeft = 0;
+}
+
+if (controllerOutputRight < 0.0){ 
+brakeRight = 1;
+    } else {
+        brakeRight = 0;
+}
+*/
+        
+      
+}
 
 int main() {
  bt.baud(BLUETOOTHBAUDRATE);
  //Sets the communication rate of the micro-controller to the Bluetooth module.
 pc.printf("Hello World!\n");
-bt.printf("Hello World!\n");'
-bt.attach(&btReceive);
- 
+bt.printf("Hello World!\n");
+
+t.start();
+float time = t.read();
+
+//bt.attach(&btReceive);
+motorLoop.attach(&PI,TI);
+
+motorLeft.period(1/freq);
+motorRight.period(1/freq);
+
+//setpointLeft = 0.0; //target speed, 0.0 to 1.0
+//setpointRight = 0.0;
+
+setpointLeft = 0.7; //target speed, 0.0 to 1.0
+setpointRight = 0.7;
+
  //prints the string to the Tera-Term Console using the Bluetooth object ‘bt’.
  while(1) {
+x = abs(acc.getAccX());
 pot1Voltage = pot1 * 3.3f;
-//.printf("Pot1 Voltage: ", pot1Voltage);
-bt.printf("Pot1 Voltage = %1.2f volts\n", pot1Voltage);
-//at full batt (12.6v) max switching freq is 58.73%
+batteryVoltage = battInput * 3.3 * battDividerScalar;
+avgCellVoltage = batteryVoltage / 3.0;
+
 //dutyCycleLeft = (pot1 * fullBatScalar);
 //dutyCycleRight = (pot1 * fullBatScalar);
 
-dutyCycleLeft = (0.5f * fullBatScalar);
-dutyCycleRight = (0.25f * fullBatScalar);
-bt.printf("Duty Cycle = %1.2f \n", dutyCycleLeft);
+//speedLeft = (speedSensorLeft * speedSensorScalar);
+//speedRight = (speedSensorRight * speedSensorScalar);
+
+speedLeftVolt = (speedSensorLeft * 3.3f);
+speedRightVolt = (speedSensorRight * 3.3f);
+
+bt.printf("Duty Cycle = %1.2f     ", dutyCycleLeft);
+bt.printf("Speed (V) L,R = %1.2f", speedLeftVolt);
+bt.printf(", %1.2f     ", speedRightVolt);
+
+bt.printf("Error L,R: %5.2f", errorLeft);
+bt.printf(", %5.2f     ", errorRight);
+
+bt.printf("ErrorArea: %1.2f     ", errorAreaLeft);
+bt.printf("Output: %1.2f     ", controllerOutputLeft);
+
+bt.printf("Batt Voltage: %1.2f \n",avgCellVoltage);
+
+//setpointLeft = pot1;
+//setpointRight = pot1;
 
 
-motorLeft.period(1/freq);
-motorRight.period(1/freq);
-motorLeft.write(dutyCycleLeft);
-motorRight.write(dutyCycleRight);
+if (t.read() > 5){
+    setpointLeft = 0.0;
+    setpointRight = 0.0;
+    }
+    
+    if (t.read_ms() > 5100){
+        setpointLeft = 0.2;
+        setpointRight = 0.2;
+        }
 
-wait(0.1);
+ if (newData){  
+      newData = false;
+  //    bt.printf("Got %c %3i\n",newInputChar, newInputInt);
+      
+      if (newInputChar == 'L')
+      setpointLeft = (newInputInt / 100.0f);
+      if (newInputChar == 'R')
+      setpointRight = (newInputInt / 100.0f);
+      
 
- }
-}
\ No newline at end of file
+//wait(0.1);
+        }
+    }
+}
+