File content as of revision 7:464fb83c8cdf:

#include "mbed.h"
//#include "Servo.h"
//#include "FastPWM.h"
#include <math.h>

Serial pc(USBTX, USBRX);
PwmOut myservo(D5);
DigitalIn servo_button_pressed(D0);

int main()
{
  myservo.period(0.02f);
  float t = 0;
  while (true)
    {
     float ref = sin(t);
     float pwm = 0.05 + 0.03 * ref;  // maximale bereik: allebei 0.05
     myservo.write(pwm);
     t+=0.02;                       // nooit groter dan periode servomotor, zelfde geldt voor regel hieronder
     wait(0.02); 
    }  
}
float theta_s_out;


void servo_horizontal()
theta_s=theta_2_h+f;         //f is de beginwaarde en theta_2_h is de waarde van INVERSE KINEM
        if(0<=theta_s<=60)
        {
            theta_s_out=0.3  //f krijgt een vaste waarde tussen de range van 0-1. Dit getal is de rangewaarde voor f   
        }    
        if (60<theta_s<180)
        {
            theta_s_out= theta_s*0.0055555556
        }
        if (theta_s=>180)
        {
            theta_s_out=1
        }
            
  
void servo_flippen()           //voor het flippen is een waarde 0.2 gekozen dit kan veranderd worden 
theta_s=theta_2_h+f;         //f is de beginwaarde en theta_2_h is de waarde van INVERSE KINEM
        if(0<=theta_s<=60)
        {
            theta_s_out=0.3-0.2  //f krijgt een vaste waarde tussen de range van 0-1. Dit getal is de rangewaarde voor f   
        }    
        if (60<theta_s<180)
        {
            theta_s_out= (theta_s*0.0055555556)-0.2
            
        }
        if (theta_s=>180)
        {
            theta_s_out=1-0.2
        }




case controlling_position:
            if (stateChanged) 
                {
                 servo_horizontal();
                 // functie waarbij de hoek gelijk blijft
                stateChanged = false;
                pc.printf("Servo hoek gelijk\r\n");
                }          
            if  (servo_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
                {      
                servo_flippen();
               //currentState = flipping_spatula;
                //stateChanged = true;
                pc.printf("Moving to flipping spatula\r\n");
                }
            if  (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
                { 
                emergency();
                } 
            break;