File content as of revision 11:51ae2da8da55:

#include "mbed.h"
#include "MODSERIAL.h"
#include "FastPWM.h"
#include "QEI.h"
#define PI 3.14159265

Serial pc(USBTX, USBRX);            //verbinden met pc
DigitalOut motor2_direction(D4);    //verbinden met motor 2 op board (altijd d4)
FastPWM motor2_pwm(D5);             //verbinden met motor 2 pwm (altijd d5)
DigitalOut motor1_direction(D6);    //verbinden met motor 2 op board (altijd d6)
FastPWM motor1_pwm(D7);             //verbinden met motor 2 pwm (altijd d7)
Ticker loop_ticker;                 //used in main()
AnalogIn Pot1(A1);                  //pot 1 op biorobotics shield
AnalogIn Pot2(A0);                  //pot 2 op biorobotics shield
AnalogIn joystick_x(A2);            //input joystick
AnalogIn joystick_y(A3);            //input joystick
InterruptIn but1(D10);              //knop 1 op birorobotics shield
InterruptIn but2(D9);               //knop 1 op birorobotics shield
QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
QEI encoder2 (D1,  D2,  NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken


//variables
enum States {idle, cw, ccw, end, failure};
States current_state;               //Definieren van de current_state met als keuzes Idle, cw, ccw, end en failure

class motor_state {                 // Een emmer met variabelen die je hierna motor gaat noemen.
    public:                         //variabelen kunnen worden gebruikt door externe functies
    float pwm1;                     //pwm of 1st motor
    float pwm2;                     //pwm of 2nd motor
    int dir1;                       //direction of 1st motor
    int dir2;                       //direction of 2nd motor
};
motor_state motor;                  //Een emmer met variabelen met motor.iets als naam bijv. motor.speed1

bool state_changed = false;         // wordt gebruikt in de state functies, om te kijken of de state net begint
volatile bool but1_pressed = false;
volatile bool but2_pressed = false;
float pot_1;
float pot_2;
float x_input;
float y_input;

/*
float x_movement = 0;
float y_movement = 0;
float angle = 0;
float extention = 0;

void calibration() {                //kalibratie van de sensoren (de nulstand invoeren)
    letter = pc.getc()              // krijg de letter vanuit de keyboard
    while (letter == w){            // of hier een while loop moet ja of nee, geen idee, lijkt me beter dan een if naar mijn mening. Eigenlijk wil je iets op de rising en falling edge hebben om zo het begin en het eind erin te kunne zetten. 
        y_movement = 1;
    }
    while (letter == a) {
        x_movement = -1;
    }
    while (letter == s){
        y_movement = -1;
    }
    while (letter == d) {
        x_movement = 1;
    }
    robot_kinematics()                  // hoe dat dit precies moet geen idee. Dit lijkt me een goed begin of iig een begin
    x_movement = 0;
    y_movement = 0;    
    
    

}
void robot_kinematics() {           //hoe de motoren moeten draaien als er een x of y richting als input is. 
    [motor.dir1, motor.dir2; motor.speed1, motor.speed2] = H^-1 *[x_movement, y_movement; angle]        // Zoiets
    
    
}
*/

// the funtions needed to run the program
void measure_signals() {
    x_input = joystick_x.read();
    y_input = joystick_y.read();

    /*
    letter = pc.getc()          // krijg de letter vanuit de keyboard
    if (letter == w){
        y_movement = 1;
    }
    if (letter == a) {
        x_movement = -1;
    }
    if (letter == s){
        y_movement = -1;
    }
    if (letter == d) {
        x_movement = 1;
    }    
    
    angle = (encoder1.getPulses()-angle_0)/8400*360; //kijkt op welke stand encoder 1 nu staat (dat hoort dus de basis motor te zijn!) /8400 signalen per rotatie * 360 graden
    extention = encoder2.getPulses()-extention_0/8400*8*5.05; //kijkt hoever de arm is uitgeschoven (/8400 per rotatie *8mm per rotatie aan verschuiving * 5.05 de gear ratio) 
    
    
    */
    
    pot_1 = Pot1.read();
    pc.printf("pot_1 = %f",pot_1);  // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
    motor.speed1 = pot_1*0.75;      //pod_read * 0.75 (dus max 75%)
    
    pot_2 = Pot2.read();
    pc.printf("pot_2 = %f",pot_2);  // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
    motor.speed2 = pot_2*0.75;      //pod_read * 0.75 (dus max 75%)
    return;
    }

void do_nothing() {
    motor.speed1 = 0;
    motor.speed2 = 0;
    
    
    //state guard
    if (but1_pressed) { //this moves the program from the idle to cw state
        current_state = cw;
        state_changed = true; //to show next state it can initialize
        pc.printf("Changed state from idle to cw\r\n");
        but1_pressed = false; //reset button 1
    }
    
}

void kinematics1() {                                                //
    double length_1;
    volatile double theta;
    volatile double length_2;
    
    class H_matrix {                                                //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
        public:
        int h1_1_1 = 1; 
        int h1_1_2 = 0;
        float h1_1_3 = sin(theta*PI/180)*(length_1+length_2);          //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
        int h1_2_1 = 0;
        int h1_2_2 = 1;
        float h1_2_3 = cos(theta*PI/180)*(length_1+length_2);  
        int h1_3_1 = 0;
        int h1_3_2 = 0;
        int h1_3_3 = 1;
                }
                H_matrix H;
                
    class Position_vector1 {                                        //positie vector gezien vanuit het uiteinde van de complete arm (coördinatiestelsel draait niet mee met de arm)
        public:
        float p1_1_1
        float p1_2_1
        float p1_3_1
                        }
    class Position_vector2 {                                         
        public:
        float p2_1_1 = h1_1_1*p1_1_1 + h1_1_2*p1_2_1 + h1_1_3*p1_3_1
        float p2_2_1 = h1_2_1*p1_1_1 + h1_2_2*p1_2_1 + h1_2_3*p1_3_1
        float p2_3_1 = h1_3_1*p1_1_1 + h1_3_2*p1_2_1 + h1_3_3*p1_3_1
        }                      
                    }


void kinematics2() {                                                //
    double length_1;
    volatile double theta;
    volatile double length_2;
    
    class H_matrix2 {                                                   //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
        public:
        int h2_1_1 = 1; 
        int h2_1_2 = 0;
        float h2_1_3 = -sin(theta*PI/180)*(length_1+length_2);         //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
        int h2_2_1 = 0;
        int h2_2_2 = 1;
        float h2_2_3 = -cos(theta*PI/180)*(length_1+length_2);  
        int h2_3_1 = 0;
        int h2_3_2 = 0;
        int h2_3_3 = 1;
                }
                H_matrix H;
                
    class Position_vector3 {                                        //positie vector gezien vanuit het onderste draaipunt
        public:
        float p3_1_1
        float p3_2_1
        float p3_3_1
                        }
    class Position_vector4 {                                         
        public:
        float p4_1_1 = h2_1_1*p3_1_1 + h2_1_2*p3_2_1 + h2_1_3*p3_3_1
        float p4_2_1 = h2_2_1*p1_1_1 + h2_2_2*p1_2_1 + h2_2_3*p3_3_1
        float p4_3_1 = h2_3_1*p1_1_1 + h2_3_2*p1_2_1 + h2_3_3*p3_3_1  
                            }
                            H_matrix2 H2
                            }
                            
void rotate_cw() {
    if (state_changed) {
        pc.printf("State changed to CW\r\n");
        state_changed = false; //reset this so it wont print next loop
    }
    motor.dir1 = 1;             //todo: check if this is actually clockwise
    motor.dir2 = 1;             //todo: check if this is actually clockwise
    
    motor.pwm1 = x_input;       //nog niet echt de x, maar de rotatie, moet later met kinematics gefixt worden
    motor.pwm2 = y_input;       // ook nog niet echt de y dus
    //state guard
    if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
        current_state = ccw;
        state_changed = true;
        but1_pressed = false; //reset this
    }
    
}
 
void rotate_ccw() {
    //similar to rotate_cw()
    if (state_changed) {
        pc.printf("State changed to CCW\r\n");
        state_changed = false;
    }
    motor.dir1 = 0;
    motor.dir2 = 0;
    
    motor.pwm1 = x_input;       //nog niet echt de x
    motor.pwm2 = y_input;       //nog niet echt de y
    
    //state guard
    if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
        current_state = cw;
        state_changed = true;
        but1_pressed = false;
    }
}
        
void motor_controller() {

    motor1_direction = motor.dir1;              // Zet de richting goed
    motor1_pwm.write(motor.pwm1);             // Zet het op de snelheid van motor.speed1
    

    motor2_direction = motor.dir2;
    motor2_pwm.write(motor.pwm2);             
    return;
}
    
void output() {return;}

void but1_interrupt () {
    but1_pressed = true;
    pc.printf("Button 1 pressed \n\r");
}

void but2_interrupt () {
    but2_pressed = true;
    pc.printf("Button 2 pressed \n\r");
}

void state_machine() {
    
    if (but2_pressed){              // Is dit de goede locatie hiervoor?
        current_state = idle;
        but2_pressed = false;
        pc.printf("Do_nothing happened due to pressing button 2 \n\r");   
    }
    
    //run current state
    switch (current_state) {
        case idle:                  // hoezo de :?
            do_nothing();
            break;
        case cw:
            rotate_cw();
            break;
        case ccw:
            rotate_ccw();
            break;
        case end:
            break;
        case failure:
            break;
    }        
}

    
void main_loop() {
    measure_signals();
    state_machine();
    motor_controller();
    output();
}

int main() {
    pc.baud(115200);
    pc.printf("Executing main()... \r\n");
    current_state = idle;
    
    motor.dir1 = 0;
    motor.dir2 = 0;
    motor2_pwm.period(0.0000625f);              // 1/frequency van waarop hij draait
    motor1_pwm.period(0.0000625f);              // 1/frequency van waarop hij draait
    
    but1.fall(&but1_interrupt);
    but2.fall(&but2_interrupt);
    loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz (niet per tiende seconde?)
    pc.printf("Main_loop is running\n \r");
    while (true) {}
}