BioRobotics - Polulu 1447 Concept version of determining the motor angle to determine tau

Dependencies:   HIDScope QEI mbed

main.cpp

Committer:
ARGroenenboom
Date:
2017-11-02
Revision:
2:d81060e7e7c6
Parent:
1:d2fe9abf5082

File content as of revision 2:d81060e7e7c6:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "iostream"

//Leds
DigitalOut ledb(LED_BLUE);
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);

//Motor
DigitalOut motor1DC(D7);
PwmOut motor1PWM(D6);
bool On;
bool reset;

//Button
DigitalIn   button1(SW2);

// HIDscope
HIDScope scope(2);

// Encoder
float X = 32;
float N = 131;
float pulse_count;
QEI Encoder(D8,D9,NC,N);

//Ticker function
Ticker tick;
double R = 30.0;
double F = 0.01;
double counter;

//Minimum wait time between rolls
//float t = 1;

// Encoder measurement function
void Measure()
{
    // Get number of pulses and calculate angle
    pulse_count = Encoder.getPulses();
    float angle_pp =360/(X * N);
    float angle = pulse_count*angle_pp;

    // Send angle to HIDScope
    scope.set(0, angle);
    scope.set(1, counter);
    scope.send();
    
    // Turn motor off if desired rotation time has been reached
    
    if(angle/360.0>=1){
        On=false;
        counter++;
        if(counter==2.0/F){
        reset=true;
        }
    }
}

//Dice rolling function
void Roll()
{  
    if(button1==0){
        On=true;
    }
    if(On==true){
        // Roll dice
        motor1PWM = 1; // motor on -> roll dice
        
        // Indicate rolling state
        ledg = 1; 
        ledb = 1; 
        ledr = 0; // red led on
        

    }
    else{
    // Stop rolling dice
        motor1PWM = 0; // motor off
        //motor1DC = abs(motor1DC-1); // rotate other way next time    
        
        // Indicate protection state
        ledr = 1; 
        ledg = 1;
        ledb = 0; // blue led on
        
    }
}

void Measurement()
{
    if(reset==true){
        Encoder.reset();
        reset = false;
        counter=0;
        }
    Roll();
    Measure();
}

int main()
{
    motor1DC = 1;
    ledr = 1;
    ledb = 1;
    ledg = 0;
    
    On = false;
    reset=true;
    counter=0;
    
    tick.attach(&Measurement,F);
    
    while(true){}
}