Biorobotics_group_2 / Mbed 2 deprecated Encoder

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope_motor_ff QEI mbed FastPWM MODSERIAL

Fork of HID_scope_test by Biorobotics_group_2

main.cpp@8:fe907b9a0bab, 2016-10-17 (annotated)

Committer:
sjoerdbarts
Date:
Mon Oct 17 09:37:52 2016 +0000
Revision:
8:fe907b9a0bab
Parent:
7:e7aa4f10d1fb
Child:
9:278d25dc0ef3
Initial working code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sjoerdbarts 0:1883abafaa19 1 #include "mbed.h"
sjoerdbarts 8:fe907b9a0bab 2 #include "FastPWM.h"
sjoerdbarts 2:5fce9d33997f 3 #include "HIDScope.h"
sjoerdbarts 5:36788b154e25 4 #include "QEI.h"
sjoerdbarts 8:fe907b9a0bab 5 #include "BiQuad.h"
sjoerdbarts 3:ce0f979f15fb 6 #define SERIAL_BAUD 115200 // baud rate for serial communication
sjoerdbarts 8:fe907b9a0bab 7
sjoerdbarts 5:36788b154e25 8 Serial pc(USBTX,USBRX);
sjoerdbarts 3:ce0f979f15fb 9
sjoerdbarts 8:fe907b9a0bab 10 // Setup Pins
sjoerdbarts 8:fe907b9a0bab 11 // Note: Pin D10 and D11 for encoder, D4-D7 for motor controller
sjoerdbarts 8:fe907b9a0bab 12 AnalogIn pot1(A0);
sjoerdbarts 8:fe907b9a0bab 13 AnalogIn pot2(A1);
sjoerdbarts 8:fe907b9a0bab 14
sjoerdbarts 8:fe907b9a0bab 15 // Setup Buttons
sjoerdbarts 8:fe907b9a0bab 16 DigitalIn button1(D2);
sjoerdbarts 8:fe907b9a0bab 17 // InterruptIn button2(D3);
sjoerdbarts 8:fe907b9a0bab 18
sjoerdbarts 8:fe907b9a0bab 19 // Set motor Pinouts
sjoerdbarts 8:fe907b9a0bab 20 DigitalOut motor1_dir(D4);
sjoerdbarts 8:fe907b9a0bab 21 FastPWM motor1_pwm(D5);
sjoerdbarts 8:fe907b9a0bab 22 //DigitalOut motor2_dir(D7);
sjoerdbarts 8:fe907b9a0bab 23 //FastPWM motor2_pwm(D6);
sjoerdbarts 8:fe907b9a0bab 24
sjoerdbarts 8:fe907b9a0bab 25 // Set LED pins
sjoerdbarts 8:fe907b9a0bab 26 DigitalOut led(LED_RED);
sjoerdbarts 8:fe907b9a0bab 27
sjoerdbarts 8:fe907b9a0bab 28 // Set HID scope
sjoerdbarts 8:fe907b9a0bab 29 HIDScope scope(2);
sjoerdbarts 8:fe907b9a0bab 30
sjoerdbarts 8:fe907b9a0bab 31 // Set encoder
sjoerdbarts 8:fe907b9a0bab 32 QEI EncoderCW(D10,D11,NC,32);
sjoerdbarts 8:fe907b9a0bab 33 QEI EncoderCCW(D11,D10,NC,32);
sjoerdbarts 8:fe907b9a0bab 34
sjoerdbarts 7:e7aa4f10d1fb 35 // Variables counter
sjoerdbarts 5:36788b154e25 36 int countsCW = 0;
sjoerdbarts 5:36788b154e25 37 int countsCCW = 0;
sjoerdbarts 5:36788b154e25 38 int net_counts = 0;
sjoerdbarts 7:e7aa4f10d1fb 39
sjoerdbarts 7:e7aa4f10d1fb 40 // Variables degrees, and speed
sjoerdbarts 5:36788b154e25 41 float degrees = 0.0;
sjoerdbarts 6:ed11342ab079 42 volatile float curr_degrees = 0.0;
sjoerdbarts 6:ed11342ab079 43 volatile float prev_degrees = 0.0;
sjoerdbarts 8:fe907b9a0bab 44 volatile float speed = 0.0; // speed in degrees/s
sjoerdbarts 8:fe907b9a0bab 45
sjoerdbarts 8:fe907b9a0bab 46 volatile const int counts_per_rev = 8400;
sjoerdbarts 8:fe907b9a0bab 47 volatile const float T_CalculateSpeed = 0.001; // 1000 Hz
sjoerdbarts 6:ed11342ab079 48
sjoerdbarts 8:fe907b9a0bab 49 // BiqUadChain
sjoerdbarts 8:fe907b9a0bab 50 BiQuadChain bqc;
sjoerdbarts 8:fe907b9a0bab 51 BiQuad bq1( 3.72805e-09, 7.45610e-09, 3.72805e-09, -1.97115e+00, 9.71392e-01 );
sjoerdbarts 8:fe907b9a0bab 52 BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98780e+00, 9.88050e-01 );
sjoerdbarts 7:e7aa4f10d1fb 53
sjoerdbarts 8:fe907b9a0bab 54
sjoerdbarts 8:fe907b9a0bab 55
sjoerdbarts 0:1883abafaa19 56
sjoerdbarts 8:fe907b9a0bab 57 float GetReferenceVelocity()
sjoerdbarts 8:fe907b9a0bab 58 {
sjoerdbarts 8:fe907b9a0bab 59 // Returns reference velocity in rad/s.
sjoerdbarts 8:fe907b9a0bab 60 // Positive value means clockwise rotation.
sjoerdbarts 8:fe907b9a0bab 61 const float maxVelocity=8.4; // in rad/s of course!
sjoerdbarts 8:fe907b9a0bab 62 float referenceVelocity; // in rad/s
sjoerdbarts 8:fe907b9a0bab 63 float button_val = button1.read();
sjoerdbarts 8:fe907b9a0bab 64 if (button1.read()) {
sjoerdbarts 8:fe907b9a0bab 65 // Clockwise rotation
sjoerdbarts 8:fe907b9a0bab 66 referenceVelocity = pot1.read()*maxVelocity;
sjoerdbarts 5:36788b154e25 67 }
sjoerdbarts 8:fe907b9a0bab 68 else {
sjoerdbarts 8:fe907b9a0bab 69 // Counterclockwise rotation
sjoerdbarts 8:fe907b9a0bab 70 referenceVelocity = -1*pot1.read()*maxVelocity;
sjoerdbarts 8:fe907b9a0bab 71 }
sjoerdbarts 8:fe907b9a0bab 72 return referenceVelocity;
sjoerdbarts 6:ed11342ab079 73 }
sjoerdbarts 5:36788b154e25 74
sjoerdbarts 8:fe907b9a0bab 75 float FeedForwardControl(float referenceVelocity)
sjoerdbarts 8:fe907b9a0bab 76 {
sjoerdbarts 8:fe907b9a0bab 77 // very simple linear feed-forward control
sjoerdbarts 8:fe907b9a0bab 78 const float MotorGain=8.4; // unit: (rad/s) / PWM
sjoerdbarts 8:fe907b9a0bab 79 float motorValue = referenceVelocity / MotorGain;
sjoerdbarts 8:fe907b9a0bab 80 pc.printf("\r\n RefVel = %f \r\n",motorValue);
sjoerdbarts 8:fe907b9a0bab 81 return motorValue;
sjoerdbarts 8:fe907b9a0bab 82 }
sjoerdbarts 8:fe907b9a0bab 83
sjoerdbarts 8:fe907b9a0bab 84 void SetMotor1(float motorValue)
sjoerdbarts 0:1883abafaa19 85 {
sjoerdbarts 8:fe907b9a0bab 86 // Given -1<=motorValue<=1, this sets the PWM and direction
sjoerdbarts 8:fe907b9a0bab 87 // bits for motor 1. Positive value makes motor rotating
sjoerdbarts 8:fe907b9a0bab 88 // clockwise. motorValues outside range are truncated to
sjoerdbarts 8:fe907b9a0bab 89 // within range
sjoerdbarts 8:fe907b9a0bab 90 if (motorValue >=0){
sjoerdbarts 8:fe907b9a0bab 91 motor1_dir=1;
sjoerdbarts 8:fe907b9a0bab 92 }
sjoerdbarts 8:fe907b9a0bab 93 else{
sjoerdbarts 8:fe907b9a0bab 94 motor1_dir=0;
sjoerdbarts 8:fe907b9a0bab 95 pc.printf("\r\n MOTORDIR = 0 \r\n");
sjoerdbarts 8:fe907b9a0bab 96 }
sjoerdbarts 8:fe907b9a0bab 97 if (fabs(motorValue)>1){
sjoerdbarts 8:fe907b9a0bab 98 motor1_pwm.write(1);
sjoerdbarts 8:fe907b9a0bab 99 }
sjoerdbarts 8:fe907b9a0bab 100 else{
sjoerdbarts 8:fe907b9a0bab 101 motor1_pwm.write(fabs(motorValue));
sjoerdbarts 8:fe907b9a0bab 102 }
sjoerdbarts 8:fe907b9a0bab 103 }
sjoerdbarts 8:fe907b9a0bab 104
sjoerdbarts 8:fe907b9a0bab 105 void MeasureAndControl(void)
sjoerdbarts 8:fe907b9a0bab 106 {
sjoerdbarts 8:fe907b9a0bab 107 // This function measures the potmeter position, extracts a
sjoerdbarts 8:fe907b9a0bab 108 // reference velocity from it, and controls the motor with
sjoerdbarts 8:fe907b9a0bab 109 // a simple FeedForward controller. Call this from a Ticker.
sjoerdbarts 8:fe907b9a0bab 110 float referenceVelocity = GetReferenceVelocity();
sjoerdbarts 8:fe907b9a0bab 111 float motorValue = FeedForwardControl(referenceVelocity);
sjoerdbarts 8:fe907b9a0bab 112 SetMotor1(motorValue);
sjoerdbarts 8:fe907b9a0bab 113 }
sjoerdbarts 8:fe907b9a0bab 114
sjoerdbarts 8:fe907b9a0bab 115 void BlinkLed(){
sjoerdbarts 8:fe907b9a0bab 116 led = not led;
sjoerdbarts 8:fe907b9a0bab 117 }
sjoerdbarts 8:fe907b9a0bab 118
sjoerdbarts 8:fe907b9a0bab 119 void CalculateSpeed() {
sjoerdbarts 5:36788b154e25 120 countsCW = EncoderCW.getPulses();
sjoerdbarts 5:36788b154e25 121 countsCCW= EncoderCCW.getPulses();
sjoerdbarts 5:36788b154e25 122 net_counts=countsCW-countsCCW;
sjoerdbarts 5:36788b154e25 123 degrees=(net_counts*360.0)/counts_per_rev;
sjoerdbarts 5:36788b154e25 124
sjoerdbarts 8:fe907b9a0bab 125 curr_degrees = degrees;
sjoerdbarts 8:fe907b9a0bab 126 speed = (curr_degrees-prev_degrees)/T_CalculateSpeed;
sjoerdbarts 8:fe907b9a0bab 127 prev_degrees = curr_degrees;
sjoerdbarts 8:fe907b9a0bab 128
sjoerdbarts 8:fe907b9a0bab 129 //scope.set(0, degrees);
sjoerdbarts 8:fe907b9a0bab 130 scope.set(0, speed);
sjoerdbarts 8:fe907b9a0bab 131 double speed_filtered = bqc.step(speed);
sjoerdbarts 8:fe907b9a0bab 132 scope.set(1,speed_filtered);
sjoerdbarts 8:fe907b9a0bab 133 scope.send();
sjoerdbarts 8:fe907b9a0bab 134 }
sjoerdbarts 8:fe907b9a0bab 135
sjoerdbarts 8:fe907b9a0bab 136 int main(){
sjoerdbarts 8:fe907b9a0bab 137 // Set baud connection with PC
sjoerdbarts 8:fe907b9a0bab 138 pc.baud(SERIAL_BAUD);
sjoerdbarts 8:fe907b9a0bab 139 pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
sjoerdbarts 8:fe907b9a0bab 140
sjoerdbarts 8:fe907b9a0bab 141 // Setup Blinking LED
sjoerdbarts 8:fe907b9a0bab 142 led = 1;
sjoerdbarts 8:fe907b9a0bab 143 Ticker TickerBlinkLed;
sjoerdbarts 8:fe907b9a0bab 144 TickerBlinkLed.attach(BlinkLed,0.5);
sjoerdbarts 8:fe907b9a0bab 145
sjoerdbarts 8:fe907b9a0bab 146 // Set motor PWM speeds
sjoerdbarts 8:fe907b9a0bab 147 motor1_pwm.period(1.0/1000);
sjoerdbarts 8:fe907b9a0bab 148 // motor2_pwm.period(1.0/1000);
sjoerdbarts 8:fe907b9a0bab 149
sjoerdbarts 8:fe907b9a0bab 150 Ticker CalculateSpeedTicker;
sjoerdbarts 8:fe907b9a0bab 151 CalculateSpeedTicker.attach(&CalculateSpeed,T_CalculateSpeed);
sjoerdbarts 8:fe907b9a0bab 152
sjoerdbarts 8:fe907b9a0bab 153 // Setup Biquad
sjoerdbarts 8:fe907b9a0bab 154 bqc.add(&bq1).add(&bq2);
sjoerdbarts 8:fe907b9a0bab 155
sjoerdbarts 8:fe907b9a0bab 156 // MeasureAndControl as fast as possible
sjoerdbarts 8:fe907b9a0bab 157 while(true){
sjoerdbarts 8:fe907b9a0bab 158 MeasureAndControl();
sjoerdbarts 5:36788b154e25 159 }
sjoerdbarts 0:1883abafaa19 160 }