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

Dependencies:   HIDScope QEI mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "QEI.h"
00004 #include "iostream"
00005 
00006 //Leds
00007 DigitalOut ledb(LED_BLUE);
00008 DigitalOut ledr(LED_RED);
00009 DigitalOut ledg(LED_GREEN);
00010 
00011 //Motor
00012 DigitalOut motor1DC(D7);
00013 PwmOut motor1PWM(D6);
00014 bool On;
00015 bool reset;
00016 
00017 //Button
00018 DigitalIn   button1(SW2);
00019 
00020 // HIDscope
00021 HIDScope scope(2);
00022 
00023 // Encoder
00024 float X = 32;
00025 float N = 131;
00026 float pulse_count;
00027 QEI Encoder(D8,D9,NC,N);
00028 
00029 //Ticker function
00030 Ticker tick;
00031 double R = 30.0;
00032 double F = 0.01;
00033 double counter;
00034 
00035 //Minimum wait time between rolls
00036 //float t = 1;
00037 
00038 // Encoder measurement function
00039 void Measure()
00040 {
00041     // Get number of pulses and calculate angle
00042     pulse_count = Encoder.getPulses();
00043     float angle_pp =360/(X * N);
00044     float angle = pulse_count*angle_pp;
00045 
00046     // Send angle to HIDScope
00047     scope.set(0, angle);
00048     scope.set(1, counter);
00049     scope.send();
00050     
00051     // Turn motor off if desired rotation time has been reached
00052     
00053     if(angle/360.0>=1){
00054         On=false;
00055         counter++;
00056         if(counter==2.0/F){
00057         reset=true;
00058         }
00059     }
00060 }
00061 
00062 //Dice rolling function
00063 void Roll()
00064 {  
00065     if(button1==0){
00066         On=true;
00067     }
00068     if(On==true){
00069         // Roll dice
00070         motor1PWM = 1; // motor on -> roll dice
00071         
00072         // Indicate rolling state
00073         ledg = 1; 
00074         ledb = 1; 
00075         ledr = 0; // red led on
00076         
00077 
00078     }
00079     else{
00080     // Stop rolling dice
00081         motor1PWM = 0; // motor off
00082         //motor1DC = abs(motor1DC-1); // rotate other way next time    
00083         
00084         // Indicate protection state
00085         ledr = 1; 
00086         ledg = 1;
00087         ledb = 0; // blue led on
00088         
00089     }
00090 }
00091 
00092 void Measurement()
00093 {
00094     if(reset==true){
00095         Encoder.reset();
00096         reset = false;
00097         counter=0;
00098         }
00099     Roll();
00100     Measure();
00101 }
00102 
00103 int main()
00104 {
00105     motor1DC = 1;
00106     ledr = 1;
00107     ledb = 1;
00108     ledg = 0;
00109     
00110     On = false;
00111     reset=true;
00112     counter=0;
00113     
00114     tick.attach(&Measurement,F);
00115     
00116     while(true){}
00117 }