Diff: MotorControl.cpp

Revision:

0:a1bb4583940a

Child:

2:0d5c994d8135

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorControl.cpp	Thu Mar 24 20:12:32 2016 +0000
@@ -0,0 +1,112 @@
+#include "TFC.h"
+#include "MotorControl.h"
+
+Motors::Motors()
+{
+    motorCurrentIndex = 0;
+    batVoltageIndex = 0;
+    
+    steeringAngle = 0;
+}
+
+void Motors::start()
+{
+    TFC_HBRIDGE_ENABLE;
+}
+
+void Motors::stop()
+{
+    TFC_HBRIDGE_DISABLE;
+}
+
+void Motors::saveBatteryVoltageMeasure(uint16_t ADCresult)
+{
+    batVoltageIndex++;
+    if(batVoltageIndex >= BATSAMPLECOUNT)
+        batVoltageIndex = 0;
+    
+    batteryVoltage[batVoltageIndex] = ADCresult;
+}
+
+void Motors::saveMotorCurrentMeasure(uint16_t MotA_ADCresult, uint16_t MotB_ADCresult)
+{
+    motorCurrentIndex++;
+    if(motorCurrentIndex >= MOTSAMPLECOUNT)
+        motorCurrentIndex = 0;
+    
+    motorACurrent[motorCurrentIndex] = MotA_ADCresult;
+    motorBCurrent[motorCurrentIndex] = MotB_ADCresult;
+}
+
+void Motors::saveSteering(float angle)
+{
+    steeringAngle = angle;
+}
+
+float Motors::getWheelRPS(char mot)
+{
+    float current,meanVoltage;
+    
+    if(mot == 'A')
+        current = (float)motorACurrent[motorCurrentIndex]/1240.9; // *3.3/4095
+    else
+        current = (float)motorBCurrent[motorCurrentIndex]/1240.9; // *3.3/4095
+    
+    meanVoltage = currentPWM*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
+    
+    // freeSpeed = 9.8004*meanVoltage-7.0023
+    // speed = (freeSpeed - (2.255*meanVoltage+10.51)*current)
+    return ((9.8004*meanVoltage-7.0023)-(2.255*meanVoltage+10.51)*current); // rps
+}
+
+float Motors::getWheelSpeed(char mot)
+{
+    return getWheelRPS(mot)*0.157;
+}
+
+void Motors::processTasks()
+{
+    
+}
+
+float Motors::getAverageMotCurrent(char mot)
+{
+    uint8_t i;
+    uint32_t sum = 0;
+    
+    if(mot == 'A')
+    {
+        for(i=0;i<MOTSAMPLECOUNT;i++)
+        {
+            sum += motorACurrent[i];
+        }
+    }
+    else
+    {
+        for(i=0;i<MOTSAMPLECOUNT;i++)
+        {
+            sum += motorBCurrent[i];
+        }
+    }
+    
+    return ((float)sum)/MOTSAMPLECOUNT/1240.9;
+}
+
+float Motors::getAverageBatteryVoltage()
+{
+    uint8_t i;
+    uint32_t sum = 0;
+    
+    for(i=0;i<BATSAMPLECOUNT;i++)
+    {
+        sum += batteryVoltage[i];
+    }
+    return ((float)sum)/BATSAMPLECOUNT/217.7;
+}
+
+
+void Motors::setFixedPWMValue(float pwm)
+{
+    currentPWM = pwm;
+    TFC_SetMotorPWM(currentPWM,currentPWM);
+}