Group 9 BioRobotics / Mbed 2 deprecated HIDScope_test

for counts to HIDScope with pot chaning motor stance

Dependencies:   HIDScope QEI biquadFilter mbed

main.cpp@0:b6853d83b8b1, 2018-10-22 (annotated)

Committer:
kweisbeek
Date:
Mon Oct 22 11:14:18 2018 +0000
Revision:
0:b6853d83b8b1
for counts to HIDScope with pot switching motor stance

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kweisbeek 0:b6853d83b8b1 1 #include "mbed.h"
kweisbeek 0:b6853d83b8b1 2 #include "HIDScope.h"
kweisbeek 0:b6853d83b8b1 3 #include "QEI.h"
kweisbeek 0:b6853d83b8b1 4
kweisbeek 0:b6853d83b8b1 5 //definitions and pin allocations
kweisbeek 0:b6853d83b8b1 6 HIDScope scope(2); // We’re going to send 2 channels of data
kweisbeek 0:b6853d83b8b1 7 Ticker AInTicker;
kweisbeek 0:b6853d83b8b1 8 //AnalogIn aIn1(A0);
kweisbeek 0:b6853d83b8b1 9 DigitalOut dirpin(D4);
kweisbeek 0:b6853d83b8b1 10 PwmOut pwmpin(D5);
kweisbeek 0:b6853d83b8b1 11 QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING); //channel A=D12, channel B=D13
kweisbeek 0:b6853d83b8b1 12 AnalogIn pot1(A1);
kweisbeek 0:b6853d83b8b1 13 AnalogIn pot2(A2);
kweisbeek 0:b6853d83b8b1 14
kweisbeek 0:b6853d83b8b1 15 //volatile variable definitions
kweisbeek 0:b6853d83b8b1 16 volatile float x;
kweisbeek 0:b6853d83b8b1 17 volatile float x_prev =0;
kweisbeek 0:b6853d83b8b1 18 volatile float y; // filtered 'output' of ReadAnalogInAndFilter
kweisbeek 0:b6853d83b8b1 19
kweisbeek 0:b6853d83b8b1 20
kweisbeek 0:b6853d83b8b1 21 //functions
kweisbeek 0:b6853d83b8b1 22 int counts(){
kweisbeek 0:b6853d83b8b1 23 int counts=Encoder.getPulses();
kweisbeek 0:b6853d83b8b1 24 return counts;}
kweisbeek 0:b6853d83b8b1 25
kweisbeek 0:b6853d83b8b1 26 float out1(){
kweisbeek 0:b6853d83b8b1 27 float out_1 = (pot1 * 2.0f) - 1.0f; //scales potmeter signal from 0 to 1 into -1 to 1
kweisbeek 0:b6853d83b8b1 28 return out_1;}
kweisbeek 0:b6853d83b8b1 29
kweisbeek 0:b6853d83b8b1 30 int out2(){
kweisbeek 0:b6853d83b8b1 31 float out_1 = (pot2 * 2.0f) - 1.0f; //scales potmeter signal from 0 to 1 into -1 to 1
kweisbeek 0:b6853d83b8b1 32 if (out_1<-0.25){
kweisbeek 0:b6853d83b8b1 33 return 0;}
kweisbeek 0:b6853d83b8b1 34
kweisbeek 0:b6853d83b8b1 35 else if ((out_1>-0.25)&&(out_1<0.25)){
kweisbeek 0:b6853d83b8b1 36 return 1;}
kweisbeek 0:b6853d83b8b1 37
kweisbeek 0:b6853d83b8b1 38 else {
kweisbeek 0:b6853d83b8b1 39 return 2;}
kweisbeek 0:b6853d83b8b1 40 }
kweisbeek 0:b6853d83b8b1 41
kweisbeek 0:b6853d83b8b1 42 void motor_pos1(){
kweisbeek 0:b6853d83b8b1 43 dirpin.write(0);
kweisbeek 0:b6853d83b8b1 44 pwmpin = 1.0f;}
kweisbeek 0:b6853d83b8b1 45
kweisbeek 0:b6853d83b8b1 46 void motor_neg1(){
kweisbeek 0:b6853d83b8b1 47 dirpin.write(1);
kweisbeek 0:b6853d83b8b1 48 pwmpin = 1.0f;}
kweisbeek 0:b6853d83b8b1 49
kweisbeek 0:b6853d83b8b1 50 void motor_pot(){
kweisbeek 0:b6853d83b8b1 51 dirpin.write((out1())<0);
kweisbeek 0:b6853d83b8b1 52 pwmpin = fabs(out1());}
kweisbeek 0:b6853d83b8b1 53
kweisbeek 0:b6853d83b8b1 54 void ReadAnalogInAndFilter(){
kweisbeek 0:b6853d83b8b1 55 x = counts(); // Capture data
kweisbeek 0:b6853d83b8b1 56 scope.set(0, x); // store data in first element of scope memory
kweisbeek 0:b6853d83b8b1 57 y = (x-x_prev)/0.001f; // averaging filter
kweisbeek 0:b6853d83b8b1 58 scope.set(1, y); // store data in second element of scope memory
kweisbeek 0:b6853d83b8b1 59 x_prev = x; // Prepare for next round
kweisbeek 0:b6853d83b8b1 60 scope.send();} // send what's in scope memory to PC
kweisbeek 0:b6853d83b8b1 61
kweisbeek 0:b6853d83b8b1 62
kweisbeek 0:b6853d83b8b1 63
kweisbeek 0:b6853d83b8b1 64
kweisbeek 0:b6853d83b8b1 65 //main function
kweisbeek 0:b6853d83b8b1 66 int main(){
kweisbeek 0:b6853d83b8b1 67 pwmpin.period_us(60);
kweisbeek 0:b6853d83b8b1 68
kweisbeek 0:b6853d83b8b1 69 //HIDScope timer attach
kweisbeek 0:b6853d83b8b1 70 AInTicker.attach(&ReadAnalogInAndFilter, 0.01);
kweisbeek 0:b6853d83b8b1 71
kweisbeek 0:b6853d83b8b1 72 //Motor control
kweisbeek 0:b6853d83b8b1 73 while (true) {
kweisbeek 0:b6853d83b8b1 74 switch(out2()){
kweisbeek 0:b6853d83b8b1 75 case 0:
kweisbeek 0:b6853d83b8b1 76 motor_pos1();
kweisbeek 0:b6853d83b8b1 77 break;
kweisbeek 0:b6853d83b8b1 78 case 1:
kweisbeek 0:b6853d83b8b1 79 motor_neg1();
kweisbeek 0:b6853d83b8b1 80 break;
kweisbeek 0:b6853d83b8b1 81 case 2:
kweisbeek 0:b6853d83b8b1 82 motor_pot();
kweisbeek 0:b6853d83b8b1 83 break;}
kweisbeek 0:b6853d83b8b1 84 wait(0.1);}
kweisbeek 0:b6853d83b8b1 85 }