File content as of revision 7:6efd4f07ae00:

/* mbed Microcontroller Library
 * Copyright (c) 2018 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "TFC.h"
#include <cmath>

DigitalOut led1(LED1);
Serial pc(USBTX, USBRX);

float lastStatusUpdate = 0;
float batteryVoltage;
void updateStatus (float delta, float currTime) {
    if (currTime - lastStatusUpdate > 1) {
        lastStatusUpdate = currTime;
        pc.printf("Battery Voltage: %.2f\r\n", batteryVoltage);
    }
}


void updateBattery (float delta, float currTime) {
    float maxVoltage = 3.0f;
    batteryVoltage = TFC_ReadBatteryVoltage();
    float fractionCharged = TFC_ReadBatteryVoltage() / maxVoltage;
    
    int level = floor(4*fractionCharged + 0.5f);
    
    if (level < 0) level = 0;
    if (level > 4) level = 4;
    
    TFC_SetBatteryLED_Level(level);
}


void update(float delta, float currTime) {
//    pc.printf("Camera1: %d; Camera2 %d\r\n", camera1.read(), camera2.read());
    TFC_HBRIDGE_ENABLE;
    TFC_SetMotorPWM(0.2f, 0.2f);
    TFC_SetServo(0, 0);
    updateBattery(delta, currTime);
    
    updateStatus(delta, currTime);
}

int main() {
    TFC_Init();
    int startClock = 0;
    float lastTime = 0;
    
    while (true) {
        
        float currTime = float(clock() - startClock) / CLOCKS_PER_SEC;
        update(currTime - lastTime, currTime);
        lastTime = currTime;
        
    }
}