Abigail Groenenboom / Mbed 2 deprecated Tau

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

Dependencies:   HIDScope QEI mbed

main.cpp@2:d81060e7e7c6, 2017-11-02 (annotated)

Committer:
ARGroenenboom
Date:
Thu Nov 02 17:24:04 2017 +0000
Revision:
2:d81060e7e7c6
Parent:
1:d2fe9abf5082 
Perfect. Niet meer aankomen.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ARGroenenboom 0:cf88b023a080 1 #include "mbed.h"
ARGroenenboom 0:cf88b023a080 2 #include "HIDScope.h"
ARGroenenboom 0:cf88b023a080 3 #include "QEI.h"
ARGroenenboom 0:cf88b023a080 4 #include "iostream"
ARGroenenboom 0:cf88b023a080 5
ARGroenenboom 0:cf88b023a080 6 //Leds
ARGroenenboom 0:cf88b023a080 7 DigitalOut ledb(LED_BLUE);
ARGroenenboom 0:cf88b023a080 8 DigitalOut ledr(LED_RED);
ARGroenenboom 0:cf88b023a080 9 DigitalOut ledg(LED_GREEN);
ARGroenenboom 0:cf88b023a080 10
ARGroenenboom 0:cf88b023a080 11 //Motor
ARGroenenboom 0:cf88b023a080 12 DigitalOut motor1DC(D7);
ARGroenenboom 0:cf88b023a080 13 PwmOut motor1PWM(D6);
ARGroenenboom 2:d81060e7e7c6 14 bool On;
ARGroenenboom 2:d81060e7e7c6 15 bool reset;
ARGroenenboom 0:cf88b023a080 16
ARGroenenboom 0:cf88b023a080 17 //Button
ARGroenenboom 0:cf88b023a080 18 DigitalIn button1(SW2);
ARGroenenboom 0:cf88b023a080 19
ARGroenenboom 0:cf88b023a080 20 // HIDscope
ARGroenenboom 2:d81060e7e7c6 21 HIDScope scope(2);
ARGroenenboom 0:cf88b023a080 22
ARGroenenboom 0:cf88b023a080 23 // Encoder
ARGroenenboom 1:d2fe9abf5082 24 float X = 32;
ARGroenenboom 1:d2fe9abf5082 25 float N = 131;
ARGroenenboom 1:d2fe9abf5082 26 float pulse_count;
ARGroenenboom 2:d81060e7e7c6 27 QEI Encoder(D8,D9,NC,N);
ARGroenenboom 2:d81060e7e7c6 28
ARGroenenboom 2:d81060e7e7c6 29 //Ticker function
ARGroenenboom 2:d81060e7e7c6 30 Ticker tick;
ARGroenenboom 2:d81060e7e7c6 31 double R = 30.0;
ARGroenenboom 2:d81060e7e7c6 32 double F = 0.01;
ARGroenenboom 2:d81060e7e7c6 33 double counter;
ARGroenenboom 0:cf88b023a080 34
ARGroenenboom 0:cf88b023a080 35 //Minimum wait time between rolls
ARGroenenboom 2:d81060e7e7c6 36 //float t = 1;
ARGroenenboom 0:cf88b023a080 37
ARGroenenboom 0:cf88b023a080 38 // Encoder measurement function
ARGroenenboom 0:cf88b023a080 39 void Measure()
ARGroenenboom 0:cf88b023a080 40 {
ARGroenenboom 2:d81060e7e7c6 41 // Get number of pulses and calculate angle
ARGroenenboom 1:d2fe9abf5082 42 pulse_count = Encoder.getPulses();
ARGroenenboom 2:d81060e7e7c6 43 float angle_pp =360/(X * N);
ARGroenenboom 2:d81060e7e7c6 44 float angle = pulse_count*angle_pp;
ARGroenenboom 2:d81060e7e7c6 45
ARGroenenboom 2:d81060e7e7c6 46 // Send angle to HIDScope
ARGroenenboom 2:d81060e7e7c6 47 scope.set(0, angle);
ARGroenenboom 2:d81060e7e7c6 48 scope.set(1, counter);
ARGroenenboom 2:d81060e7e7c6 49 scope.send();
ARGroenenboom 1:d2fe9abf5082 50
ARGroenenboom 2:d81060e7e7c6 51 // Turn motor off if desired rotation time has been reached
ARGroenenboom 0:cf88b023a080 52
ARGroenenboom 2:d81060e7e7c6 53 if(angle/360.0>=1){
ARGroenenboom 2:d81060e7e7c6 54 On=false;
ARGroenenboom 2:d81060e7e7c6 55 counter++;
ARGroenenboom 2:d81060e7e7c6 56 if(counter==2.0/F){
ARGroenenboom 2:d81060e7e7c6 57 reset=true;
ARGroenenboom 2:d81060e7e7c6 58 }
ARGroenenboom 2:d81060e7e7c6 59 }
ARGroenenboom 0:cf88b023a080 60 }
ARGroenenboom 0:cf88b023a080 61
ARGroenenboom 0:cf88b023a080 62 //Dice rolling function
ARGroenenboom 0:cf88b023a080 63 void Roll()
ARGroenenboom 0:cf88b023a080 64 {
ARGroenenboom 2:d81060e7e7c6 65 if(button1==0){
ARGroenenboom 2:d81060e7e7c6 66 On=true;
ARGroenenboom 2:d81060e7e7c6 67 }
ARGroenenboom 2:d81060e7e7c6 68 if(On==true){
ARGroenenboom 2:d81060e7e7c6 69 // Roll dice
ARGroenenboom 2:d81060e7e7c6 70 motor1PWM = 1; // motor on -> roll dice
ARGroenenboom 2:d81060e7e7c6 71
ARGroenenboom 2:d81060e7e7c6 72 // Indicate rolling state
ARGroenenboom 2:d81060e7e7c6 73 ledg = 1;
ARGroenenboom 2:d81060e7e7c6 74 ledb = 1;
ARGroenenboom 2:d81060e7e7c6 75 ledr = 0; // red led on
ARGroenenboom 2:d81060e7e7c6 76
ARGroenenboom 0:cf88b023a080 77
ARGroenenboom 2:d81060e7e7c6 78 }
ARGroenenboom 2:d81060e7e7c6 79 else{
ARGroenenboom 2:d81060e7e7c6 80 // Stop rolling dice
ARGroenenboom 2:d81060e7e7c6 81 motor1PWM = 0; // motor off
ARGroenenboom 2:d81060e7e7c6 82 //motor1DC = abs(motor1DC-1); // rotate other way next time
ARGroenenboom 2:d81060e7e7c6 83
ARGroenenboom 2:d81060e7e7c6 84 // Indicate protection state
ARGroenenboom 2:d81060e7e7c6 85 ledr = 1;
ARGroenenboom 2:d81060e7e7c6 86 ledg = 1;
ARGroenenboom 2:d81060e7e7c6 87 ledb = 0; // blue led on
ARGroenenboom 2:d81060e7e7c6 88
ARGroenenboom 2:d81060e7e7c6 89 }
ARGroenenboom 2:d81060e7e7c6 90 }
ARGroenenboom 2:d81060e7e7c6 91
ARGroenenboom 2:d81060e7e7c6 92 void Measurement()
ARGroenenboom 2:d81060e7e7c6 93 {
ARGroenenboom 2:d81060e7e7c6 94 if(reset==true){
ARGroenenboom 2:d81060e7e7c6 95 Encoder.reset();
ARGroenenboom 2:d81060e7e7c6 96 reset = false;
ARGroenenboom 2:d81060e7e7c6 97 counter=0;
ARGroenenboom 2:d81060e7e7c6 98 }
ARGroenenboom 2:d81060e7e7c6 99 Roll();
ARGroenenboom 2:d81060e7e7c6 100 Measure();
ARGroenenboom 0:cf88b023a080 101 }
ARGroenenboom 0:cf88b023a080 102
ARGroenenboom 0:cf88b023a080 103 int main()
ARGroenenboom 0:cf88b023a080 104 {
ARGroenenboom 0:cf88b023a080 105 motor1DC = 1;
ARGroenenboom 2:d81060e7e7c6 106 ledr = 1;
ARGroenenboom 0:cf88b023a080 107 ledb = 1;
ARGroenenboom 0:cf88b023a080 108 ledg = 0;
ARGroenenboom 0:cf88b023a080 109
ARGroenenboom 2:d81060e7e7c6 110 On = false;
ARGroenenboom 2:d81060e7e7c6 111 reset=true;
ARGroenenboom 2:d81060e7e7c6 112 counter=0;
ARGroenenboom 2:d81060e7e7c6 113
ARGroenenboom 2:d81060e7e7c6 114 tick.attach(&Measurement,F);
ARGroenenboom 2:d81060e7e7c6 115
ARGroenenboom 2:d81060e7e7c6 116 while(true){}
ARGroenenboom 0:cf88b023a080 117 }