BioRobotics - Polulu 1447 Concept version of determining the motor angle to determine tau
Dependencies: HIDScope QEI mbed
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 }
Generated on Tue Jul 12 2022 16:57:25 by
1.7.2