File content as of revision 3:32afe87d4b62:

#include "OfflineCommand.h"

volatile int32_t OfflineCommand::addr = 0;

OfflineCommand::OfflineCommand(Sensors* sensors, Storage* storage)
{
    this->sensors = sensors;
    this->storage = storage;
    addr = 0x0;
    this->running = 0;
    this->state = 0;
}

void OfflineCommand::initialize()
{
    printf("OfflineCommand\n");
    this->state = 0;
    setTicker(0.1);
}

void OfflineCommand::execute()
{
    if(running) {
        writeToMemory();
    }
}

void OfflineCommand::button()
{
    switch(state) {
        case 0:
        printf("ERASE COMMAND\n");
            detachTicker();
            setLed(1);
            addr = 0;
            wait(0.1);
            eraseMemory();
            this->state = 1;
            setTicker(0.1);
            break;
        case 1:
            detachTicker();
            setLed(1);
            printf("WRITE COMMAND\n");
            addr = 0;
            this->running = 1; //This will unlock the writeToMemory from the execution
            this->state = 2;
            this->sensors->wakeIMU();
            break;
        case 2:
            setTicker(0.1);
            printf("DONE WRITE COMMAND...");
            printf("%d\n", addr);
            this->sensors->disableIMU();
            this->running = 0;
            this->state = 0;
            break;
    }

}

void OfflineCommand::finish()
{
    this->state = 0;
    detachTicker();
    setLed(0);
}

void OfflineCommand::writeToMemory()
{
    sensors->updateAngle();
    sensors->updateIMU();
    
    if(!storage->isBusy()) {

        storage->pageProgram(addr);

        storage->writeData(sensors->getAngle(0));
        storage->writeData(sensors->getAngle(1));
        storage->writeData(sensors->getIMU(0));
        storage->writeData(sensors->getIMU(1));
        
        storage->endAction();

        addr = addr + 0x4;

    } else {
        printf("busy: %d\n", storage->getSR1());
    }

}

void OfflineCommand::eraseMemory()
{
    /*
    __disable_irq(); 
    storage->reset();
    
    while(storage->isBusy()) {
        wait(0.1);
    }
    
    storage->setWREN();
    wait(0.01);
    storage->bulkErase();
    wait(0.01);
    
    printf("erasing...\n");

    while(storage->isBusy()) {
        wait(0.1);
    }

    printf("SUCCES!\n");
    __enable_irq(); 
    */

    __disable_irq(); 

    storage->read(0x00);
    
    while(storage->readData() != -1){
        storage->endAction(); //end previous read action
        
        storage->sectorErase(addr);
        
        while(storage->isBusy()){ wait(0.01); }
    
        printf("sector %d erased\n", addr);
        addr = addr + (1<<18);
        storage->read(addr);   //set next section addr for read in the while statement
    }
    
    printf("ERASE done!\n");
    storage->endAction();
    
     __enable_irq(); 
}