Juggler Position Control Parsing

Dependencies:   MODSERIAL mbed Servo

Fork of juggler_mbed_position_control by Robert Katzschmann

main.cpp

Committer:
Symplectomorphism
Date:
2017-01-15
Revision:
1:153ce28ac5ed
Parent:
0:1b69f8402884
Child:
2:b4aef96e0cd7

File content as of revision 1:153ce28ac5ed:

#include "mbed.h"
#include "MODSERIAL/MODSERIAL.h"
#include "SerialComm/SerialComm.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#define NUM_FLOATS 10


MODSERIAL pcSerial(USBTX, USBRX);

// Initialize a pins t//o perform analog and digital output fucntions
AnalogOut  aout(p18);
PwmOut led1(LED1);
// DigitalOut led2(LED2);
//DigitalOut led3(LED3);
Ticker aTimer;

//Serial pc(USBTX, USBRX);
float valueFloats[NUM_FLOATS];
float voltage;

SerialComm serialComm(&pcSerial);

bool readyToSendNext;

void timerFcn()
{
    readyToSendNext = true;
}


int main(void)
{
    
    float fReceived = 0;
    float fToSend = 0;
    pcSerial.baud(115200);
    pcSerial.printf("Start!\n");
    
    /*
    while(true) {
        if(serialComm.checkIfNewMessage()) {
            valueFloats[0] = serialComm.getFloat();//valueFloats, NUM_FLOATS);
            led2 = !led2;

            voltage = (abs(valueFloats[0]) > 3.3f) ? 3.3f : abs(valueFloats[0]);
            aout = voltage / 3.3f;
            // turn on the led if the voltage is greater than 0.5f * VCC
            led1 = (aout > 0.5f) ? 1 : 0; // voltage larger than 0.5*3.3

        }
        wait(1.0f);
    }*/
    //int i = 0;
    readyToSendNext = false;
    aTimer.attach(&timerFcn, 0.002); // the address of the function to be attached (flip) and the interval (2 seconds)

    while(1) {
        if(serialComm.checkIfNewMessage()) 
        {
            fReceived = serialComm.getFloat();
        }
//        
        if(readyToSendNext == true)
        {
            fToSend = fReceived; // / 3.3f;
            //led1 = fToSend; // voltage larger than 0.5*3.3
            
            //i++;
            // led1 = (ftemp > 0.5f) ? !led1 : 0; // voltage larger than 0.5*3.3
            serialComm.sendFloat(fToSend); // write it back
            readyToSendNext = false;    
        }
        
        // float testFloat = 0.1*i;
        
             
        //wait(0.01f);
    }
}