Pascal Sandrez / MotorControl

Custom version for NXP cup car

Dependents:   NXPCUPcar

MotorControl.cpp@0:a1bb4583940a, 2016-03-24 (annotated)

Committer:
Clarkk
Date:
Thu Mar 24 20:12:32 2016 +0000
Revision:
0:a1bb4583940a
Child:
2:0d5c994d8135
First version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Clarkk 0:a1bb4583940a 1 #include "TFC.h"
Clarkk 0:a1bb4583940a 2 #include "MotorControl.h"
Clarkk 0:a1bb4583940a 3
Clarkk 0:a1bb4583940a 4 Motors::Motors()
Clarkk 0:a1bb4583940a 5 {
Clarkk 0:a1bb4583940a 6 motorCurrentIndex = 0;
Clarkk 0:a1bb4583940a 7 batVoltageIndex = 0;
Clarkk 0:a1bb4583940a 8
Clarkk 0:a1bb4583940a 9 steeringAngle = 0;
Clarkk 0:a1bb4583940a 10 }
Clarkk 0:a1bb4583940a 11
Clarkk 0:a1bb4583940a 12 void Motors::start()
Clarkk 0:a1bb4583940a 13 {
Clarkk 0:a1bb4583940a 14 TFC_HBRIDGE_ENABLE;
Clarkk 0:a1bb4583940a 15 }
Clarkk 0:a1bb4583940a 16
Clarkk 0:a1bb4583940a 17 void Motors::stop()
Clarkk 0:a1bb4583940a 18 {
Clarkk 0:a1bb4583940a 19 TFC_HBRIDGE_DISABLE;
Clarkk 0:a1bb4583940a 20 }
Clarkk 0:a1bb4583940a 21
Clarkk 0:a1bb4583940a 22 void Motors::saveBatteryVoltageMeasure(uint16_t ADCresult)
Clarkk 0:a1bb4583940a 23 {
Clarkk 0:a1bb4583940a 24 batVoltageIndex++;
Clarkk 0:a1bb4583940a 25 if(batVoltageIndex >= BATSAMPLECOUNT)
Clarkk 0:a1bb4583940a 26 batVoltageIndex = 0;
Clarkk 0:a1bb4583940a 27
Clarkk 0:a1bb4583940a 28 batteryVoltage[batVoltageIndex] = ADCresult;
Clarkk 0:a1bb4583940a 29 }
Clarkk 0:a1bb4583940a 30
Clarkk 0:a1bb4583940a 31 void Motors::saveMotorCurrentMeasure(uint16_t MotA_ADCresult, uint16_t MotB_ADCresult)
Clarkk 0:a1bb4583940a 32 {
Clarkk 0:a1bb4583940a 33 motorCurrentIndex++;
Clarkk 0:a1bb4583940a 34 if(motorCurrentIndex >= MOTSAMPLECOUNT)
Clarkk 0:a1bb4583940a 35 motorCurrentIndex = 0;
Clarkk 0:a1bb4583940a 36
Clarkk 0:a1bb4583940a 37 motorACurrent[motorCurrentIndex] = MotA_ADCresult;
Clarkk 0:a1bb4583940a 38 motorBCurrent[motorCurrentIndex] = MotB_ADCresult;
Clarkk 0:a1bb4583940a 39 }
Clarkk 0:a1bb4583940a 40
Clarkk 0:a1bb4583940a 41 void Motors::saveSteering(float angle)
Clarkk 0:a1bb4583940a 42 {
Clarkk 0:a1bb4583940a 43 steeringAngle = angle;
Clarkk 0:a1bb4583940a 44 }
Clarkk 0:a1bb4583940a 45
Clarkk 0:a1bb4583940a 46 float Motors::getWheelRPS(char mot)
Clarkk 0:a1bb4583940a 47 {
Clarkk 0:a1bb4583940a 48 float current,meanVoltage;
Clarkk 0:a1bb4583940a 49
Clarkk 0:a1bb4583940a 50 if(mot == 'A')
Clarkk 0:a1bb4583940a 51 current = (float)motorACurrent[motorCurrentIndex]/1240.9; // *3.3/4095
Clarkk 0:a1bb4583940a 52 else
Clarkk 0:a1bb4583940a 53 current = (float)motorBCurrent[motorCurrentIndex]/1240.9; // *3.3/4095
Clarkk 0:a1bb4583940a 54
Clarkk 0:a1bb4583940a 55 meanVoltage = currentPWM*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
Clarkk 0:a1bb4583940a 56
Clarkk 0:a1bb4583940a 57 // freeSpeed = 9.8004*meanVoltage-7.0023
Clarkk 0:a1bb4583940a 58 // speed = (freeSpeed - (2.255*meanVoltage+10.51)*current)
Clarkk 0:a1bb4583940a 59 return ((9.8004*meanVoltage-7.0023)-(2.255*meanVoltage+10.51)*current); // rps
Clarkk 0:a1bb4583940a 60 }
Clarkk 0:a1bb4583940a 61
Clarkk 0:a1bb4583940a 62 float Motors::getWheelSpeed(char mot)
Clarkk 0:a1bb4583940a 63 {
Clarkk 0:a1bb4583940a 64 return getWheelRPS(mot)*0.157;
Clarkk 0:a1bb4583940a 65 }
Clarkk 0:a1bb4583940a 66
Clarkk 0:a1bb4583940a 67 void Motors::processTasks()
Clarkk 0:a1bb4583940a 68 {
Clarkk 0:a1bb4583940a 69
Clarkk 0:a1bb4583940a 70 }
Clarkk 0:a1bb4583940a 71
Clarkk 0:a1bb4583940a 72 float Motors::getAverageMotCurrent(char mot)
Clarkk 0:a1bb4583940a 73 {
Clarkk 0:a1bb4583940a 74 uint8_t i;
Clarkk 0:a1bb4583940a 75 uint32_t sum = 0;
Clarkk 0:a1bb4583940a 76
Clarkk 0:a1bb4583940a 77 if(mot == 'A')
Clarkk 0:a1bb4583940a 78 {
Clarkk 0:a1bb4583940a 79 for(i=0;i<MOTSAMPLECOUNT;i++)
Clarkk 0:a1bb4583940a 80 {
Clarkk 0:a1bb4583940a 81 sum += motorACurrent[i];
Clarkk 0:a1bb4583940a 82 }
Clarkk 0:a1bb4583940a 83 }
Clarkk 0:a1bb4583940a 84 else
Clarkk 0:a1bb4583940a 85 {
Clarkk 0:a1bb4583940a 86 for(i=0;i<MOTSAMPLECOUNT;i++)
Clarkk 0:a1bb4583940a 87 {
Clarkk 0:a1bb4583940a 88 sum += motorBCurrent[i];
Clarkk 0:a1bb4583940a 89 }
Clarkk 0:a1bb4583940a 90 }
Clarkk 0:a1bb4583940a 91
Clarkk 0:a1bb4583940a 92 return ((float)sum)/MOTSAMPLECOUNT/1240.9;
Clarkk 0:a1bb4583940a 93 }
Clarkk 0:a1bb4583940a 94
Clarkk 0:a1bb4583940a 95 float Motors::getAverageBatteryVoltage()
Clarkk 0:a1bb4583940a 96 {
Clarkk 0:a1bb4583940a 97 uint8_t i;
Clarkk 0:a1bb4583940a 98 uint32_t sum = 0;
Clarkk 0:a1bb4583940a 99
Clarkk 0:a1bb4583940a 100 for(i=0;i<BATSAMPLECOUNT;i++)
Clarkk 0:a1bb4583940a 101 {
Clarkk 0:a1bb4583940a 102 sum += batteryVoltage[i];
Clarkk 0:a1bb4583940a 103 }
Clarkk 0:a1bb4583940a 104 return ((float)sum)/BATSAMPLECOUNT/217.7;
Clarkk 0:a1bb4583940a 105 }
Clarkk 0:a1bb4583940a 106
Clarkk 0:a1bb4583940a 107
Clarkk 0:a1bb4583940a 108 void Motors::setFixedPWMValue(float pwm)
Clarkk 0:a1bb4583940a 109 {
Clarkk 0:a1bb4583940a 110 currentPWM = pwm;
Clarkk 0:a1bb4583940a 111 TFC_SetMotorPWM(currentPWM,currentPWM);
Clarkk 0:a1bb4583940a 112 }