First Last / Mbed 2 deprecated BMT-M9_motorcontrol

MOtor control Pololu

Dependencies:   Encoder HIDScope mbed-dsp mbed

main.cpp@4:1a53b06eeb7f, 2014-09-29 (annotated)

Committer:
vsluiter
Date:
Mon Sep 29 21:07:58 2014 +0000
Revision:
4:1a53b06eeb7f
Parent:
2:5f5b229b004d
Child:
5:06381e54b94a
Updated Encoder;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c9e647421e54 1 #include "mbed.h"
vsluiter 0:c9e647421e54 2 #include "encoder.h"
vsluiter 0:c9e647421e54 3 #include "MODSERIAL.h"
vsluiter 0:c9e647421e54 4
vsluiter 4:1a53b06eeb7f 5 #define K_P .01
vsluiter 4:1a53b06eeb7f 6 //0.007//0.0184997836671646 //0.015
vsluiter 4:1a53b06eeb7f 7 #define K_I 0.00
vsluiter 4:1a53b06eeb7f 8 //.000824387821287097 //0
vsluiter 4:1a53b06eeb7f 9 #define K_D .05
vsluiter 4:1a53b06eeb7f 10 //0.01//.0972091946803081 //0.077
vsluiter 1:5ac85aad9da4 11
vsluiter 4:1a53b06eeb7f 12 #define POT_AVG 50
vsluiter 0:c9e647421e54 13 void coerce(float * in, float min, float max);
vsluiter 0:c9e647421e54 14 float pid(float setpoint, float measurement);
vsluiter 1:5ac85aad9da4 15 AnalogIn potmeter(PTC2);
vsluiter 0:c9e647421e54 16 volatile bool looptimerflag;
vsluiter 1:5ac85aad9da4 17 float potsamples[POT_AVG];
vsluiter 0:c9e647421e54 18
vsluiter 0:c9e647421e54 19 void setlooptimerflag(void)
vsluiter 0:c9e647421e54 20 {
vsluiter 0:c9e647421e54 21 looptimerflag = true;
vsluiter 0:c9e647421e54 22 }
vsluiter 0:c9e647421e54 23
vsluiter 1:5ac85aad9da4 24 void potAverager(void)
vsluiter 1:5ac85aad9da4 25 {
vsluiter 1:5ac85aad9da4 26
vsluiter 1:5ac85aad9da4 27 static uint16_t sample_index = 0;
vsluiter 1:5ac85aad9da4 28 float voltage = potmeter.read()-.5;
vsluiter 1:5ac85aad9da4 29
vsluiter 1:5ac85aad9da4 30 potsamples[sample_index] = voltage;
vsluiter 4:1a53b06eeb7f 31
vsluiter 1:5ac85aad9da4 32 sample_index++;
vsluiter 1:5ac85aad9da4 33 if(sample_index >= POT_AVG)
vsluiter 1:5ac85aad9da4 34 sample_index = 0;
vsluiter 1:5ac85aad9da4 35 }
vsluiter 1:5ac85aad9da4 36
vsluiter 4:1a53b06eeb7f 37 void bubblesort(float arr[], int size)
vsluiter 4:1a53b06eeb7f 38 {
vsluiter 4:1a53b06eeb7f 39 int i,j;
vsluiter 4:1a53b06eeb7f 40 float temp;
vsluiter 4:1a53b06eeb7f 41 for ( i = 0 ; i <= (size/2)+1 ; i++ )
vsluiter 4:1a53b06eeb7f 42 {
vsluiter 4:1a53b06eeb7f 43 for ( j = 0 ; j <= (size/2)+1 - i ; j++ )
vsluiter 4:1a53b06eeb7f 44 {
vsluiter 4:1a53b06eeb7f 45 if ( arr[j] > arr[j + 1] )
vsluiter 4:1a53b06eeb7f 46 {
vsluiter 4:1a53b06eeb7f 47 temp = arr[j] ;
vsluiter 4:1a53b06eeb7f 48 arr[j] = arr[j + 1] ;
vsluiter 4:1a53b06eeb7f 49 arr[j + 1] = temp ;
vsluiter 4:1a53b06eeb7f 50 }
vsluiter 4:1a53b06eeb7f 51 }
vsluiter 4:1a53b06eeb7f 52 }
vsluiter 4:1a53b06eeb7f 53 }
vsluiter 4:1a53b06eeb7f 54
vsluiter 4:1a53b06eeb7f 55 void quicksort(float own_array[], int low, int high)
vsluiter 4:1a53b06eeb7f 56 {
vsluiter 4:1a53b06eeb7f 57 int i = low;
vsluiter 4:1a53b06eeb7f 58 int j = high;
vsluiter 4:1a53b06eeb7f 59 int y = 0;
vsluiter 4:1a53b06eeb7f 60 /* compare value */
vsluiter 4:1a53b06eeb7f 61 int z = own_array[(low + high) / 2];
vsluiter 4:1a53b06eeb7f 62 /* partition */
vsluiter 4:1a53b06eeb7f 63 do {
vsluiter 4:1a53b06eeb7f 64 /* find member above ... */
vsluiter 4:1a53b06eeb7f 65 while(own_array[i] < z) i++;
vsluiter 4:1a53b06eeb7f 66
vsluiter 4:1a53b06eeb7f 67 /* find element below ... */
vsluiter 4:1a53b06eeb7f 68 while(own_array[j] > z) j--;
vsluiter 4:1a53b06eeb7f 69
vsluiter 4:1a53b06eeb7f 70 if(i <= j) {
vsluiter 4:1a53b06eeb7f 71 /* swap two elements */
vsluiter 4:1a53b06eeb7f 72 y = own_array[i];
vsluiter 4:1a53b06eeb7f 73 own_array[i] = own_array[j];
vsluiter 4:1a53b06eeb7f 74 own_array[j] = y;
vsluiter 4:1a53b06eeb7f 75 i++;
vsluiter 4:1a53b06eeb7f 76 j--;
vsluiter 4:1a53b06eeb7f 77 }
vsluiter 4:1a53b06eeb7f 78 } while(i <= j);
vsluiter 4:1a53b06eeb7f 79
vsluiter 4:1a53b06eeb7f 80 /* recurse */
vsluiter 4:1a53b06eeb7f 81 if(low <= j)
vsluiter 4:1a53b06eeb7f 82 quicksort(own_array, low, j);
vsluiter 4:1a53b06eeb7f 83
vsluiter 4:1a53b06eeb7f 84 if(i <= high)
vsluiter 4:1a53b06eeb7f 85 quicksort(own_array, i, high);
vsluiter 4:1a53b06eeb7f 86 }
vsluiter 4:1a53b06eeb7f 87
vsluiter 1:5ac85aad9da4 88 float getpotAverage(void)
vsluiter 1:5ac85aad9da4 89 {
vsluiter 4:1a53b06eeb7f 90 //float own_array[POT_AVG];
vsluiter 4:1a53b06eeb7f 91 //memcpy(own_array, potsamples, sizeof(own_array));
vsluiter 4:1a53b06eeb7f 92 //bubblesort(own_array, POT_AVG-1);
vsluiter 4:1a53b06eeb7f 93 //return own_array[POT_AVG/2];
vsluiter 4:1a53b06eeb7f 94 uint16_t valuecounter;
vsluiter 4:1a53b06eeb7f 95 float sum = 0;
vsluiter 4:1a53b06eeb7f 96 for(valuecounter = 0 ; valuecounter < POT_AVG ; valuecounter++)
vsluiter 4:1a53b06eeb7f 97 {
vsluiter 4:1a53b06eeb7f 98 sum += potsamples[valuecounter];
vsluiter 4:1a53b06eeb7f 99 }
vsluiter 1:5ac85aad9da4 100 return sum / (POT_AVG*1.);
vsluiter 1:5ac85aad9da4 101 }
vsluiter 1:5ac85aad9da4 102
vsluiter 4:1a53b06eeb7f 103 int main()
vsluiter 4:1a53b06eeb7f 104 {
vsluiter 0:c9e647421e54 105 Encoder motor1(PTD0,PTC9);
vsluiter 1:5ac85aad9da4 106 Ticker potaverage;
vsluiter 0:c9e647421e54 107 MODSERIAL pc(USBTX,USBRX);
vsluiter 0:c9e647421e54 108 PwmOut pwm_motor(PTA12);
vsluiter 0:c9e647421e54 109 DigitalOut motordir(PTD3);
vsluiter 1:5ac85aad9da4 110 potaverage.attach(potAverager,0.002);
vsluiter 4:1a53b06eeb7f 111 pc.baud(921600);
vsluiter 0:c9e647421e54 112 Ticker looptimer;
vsluiter 4:1a53b06eeb7f 113 looptimer.attach(setlooptimerflag,0.01);
vsluiter 4:1a53b06eeb7f 114 pc.printf("bla");
vsluiter 0:c9e647421e54 115 while(1) {
vsluiter 1:5ac85aad9da4 116 float setpoint;
vsluiter 0:c9e647421e54 117 float new_pwm;
vsluiter 0:c9e647421e54 118 while(!looptimerflag);
vsluiter 0:c9e647421e54 119 looptimerflag = false;
vsluiter 1:5ac85aad9da4 120 setpoint = (getpotAverage())*2000;
vsluiter 2:5f5b229b004d 121 //new_pwm = (setpoint - motor1.getPosition())*.001;
vsluiter 2:5f5b229b004d 122 new_pwm = pid(setpoint, motor1.getPosition());
vsluiter 0:c9e647421e54 123 coerce(&new_pwm, -1,1);
vsluiter 4:1a53b06eeb7f 124 pc.printf("%f %f %f 0\n", setpoint/2000.+0.5, motor1.getPosition()/2000.+.5,new_pwm/2.+0.5);
vsluiter 0:c9e647421e54 125 if(new_pwm > 0)
vsluiter 0:c9e647421e54 126 motordir = 1;
vsluiter 0:c9e647421e54 127 else
vsluiter 4:1a53b06eeb7f 128 motordir = 0;
vsluiter 0:c9e647421e54 129 pwm_motor.write(abs(new_pwm));
vsluiter 0:c9e647421e54 130 }
vsluiter 0:c9e647421e54 131 }
vsluiter 0:c9e647421e54 132
vsluiter 1:5ac85aad9da4 133
vsluiter 1:5ac85aad9da4 134 //coerces value 'in' to min or max when exceeding those values
vsluiter 1:5ac85aad9da4 135 //if you'd like to understand the statement below take a google for
vsluiter 1:5ac85aad9da4 136 //'ternary operators'.
vsluiter 0:c9e647421e54 137 void coerce(float * in, float min, float max)
vsluiter 0:c9e647421e54 138 {
vsluiter 4:1a53b06eeb7f 139 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:c9e647421e54 140 }
vsluiter 0:c9e647421e54 141
vsluiter 0:c9e647421e54 142
vsluiter 0:c9e647421e54 143 float pid(float setpoint, float measurement)
vsluiter 0:c9e647421e54 144 {
vsluiter 4:1a53b06eeb7f 145 float error;
vsluiter 4:1a53b06eeb7f 146 static float prev_error = 0;
vsluiter 4:1a53b06eeb7f 147 float out_p = 0;
vsluiter 4:1a53b06eeb7f 148 static float out_i = 0;
vsluiter 4:1a53b06eeb7f 149 float out_d = 0;
vsluiter 4:1a53b06eeb7f 150 error = setpoint-measurement;
vsluiter 4:1a53b06eeb7f 151 out_p = error*K_P;
vsluiter 4:1a53b06eeb7f 152 out_i += error*K_I;
vsluiter 4:1a53b06eeb7f 153 out_d = (error-prev_error)*K_D;
vsluiter 4:1a53b06eeb7f 154 coerce(&out_i,-0.5,0.5);
vsluiter 4:1a53b06eeb7f 155 prev_error = error;
vsluiter 4:1a53b06eeb7f 156 return out_p + out_i + out_d;
vsluiter 0:c9e647421e54 157 }
vsluiter 0:c9e647421e54 158