Diff: main.cpp

Revision:

3:25c6bf0abc00

Parent:

2:c857935f928e

Child:

6:d2bd68ba99c9

--- a/main.cpp	Mon Oct 25 01:28:53 2021 +0000
+++ b/main.cpp	Mon Oct 25 01:39:55 2021 +0000
@@ -5,15 +5,11 @@
 #include "state_control.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
-
+#include "bluetooth.h"
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
 Serial pc(USBTX, USBRX); // tx, rx
-Serial bt(SERIALTXPORT, SERIALRXPORT); //(TX, RX) Serial declaration for new serial
+
 
 PinName const SDA = PTE25;
 PinName const SCL = PTE24;
@@ -22,203 +18,7 @@
 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;
-
-
-//--- 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);
-
-
-//--- motor braking ---
-/*
-if (controllerOutputLeft < 0.0){ 
-brakeLeft = 1;
-    } else {
-        brakeLeft = 0;
-}
-
-if (controllerOutputRight < 0.0){ 
-brakeRight = 1;
-    } else {
-        brakeRight = 0;
-}
-*/
-        
-      
-}
-
-int main() {
- 
-
+int main() { 
     state_update();
    
     //Delcare Onboard LED with blue color