Bas van Buuren / Mbed 2 deprecated motor_input

Functie van het slaan van de bal

Dependencies:   Encoder HIDScope mbed-dsp mbed MODSERIAL

main.cpp@14:e9ab81429271, 2014-10-29 (annotated)

Committer:
BasvanBuuren
Date:
Wed Oct 29 16:21:19 2014 +0000
Revision:
14:e9ab81429271
Parent:
13:b35f3553f210 
Hij doet het gewoon!

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BasvanBuuren 0:8af618dc161b 1 #include "mbed.h"
BasvanBuuren 0:8af618dc161b 2 #include "HIDScope.h"
BasvanBuuren 0:8af618dc161b 3 //#include "mbed-dsp.h"
BasvanBuuren 0:8af618dc161b 4 #include "encoder.h"
BasvanBuuren 14:e9ab81429271 5 //#include <iostream>
BasvanBuuren 14:e9ab81429271 6 #include "MODSERIAL.h"
BasvanBuuren 0:8af618dc161b 7
BasvanBuuren 6:3c4a2afb11e5 8 #define SAMP_TIME 0.01
BasvanBuuren 13:b35f3553f210 9 #define K_P (0.001)
BasvanBuuren 13:b35f3553f210 10 #define K_I (0.000001 *SAMP_TIME)
BasvanBuuren 13:b35f3553f210 11 #define K_D (0.000005 /SAMP_TIME)
BasvanBuuren 5:daf561abb67d 12 #define I_LIMIT 1.
BasvanBuuren 5:daf561abb67d 13
BasvanBuuren 13:b35f3553f210 14 #define MAXENCO 320
BasvanBuuren 0:8af618dc161b 15 #define MINENCO 0
BasvanBuuren 0:8af618dc161b 16
BasvanBuuren 5:daf561abb67d 17
BasvanBuuren 5:daf561abb67d 18 //define in and output
BasvanBuuren 14:e9ab81429271 19 MODSERIAL pc(USBTX,USBRX);
BasvanBuuren 0:8af618dc161b 20 Encoder encoderA(PTD0,PTD2);
BasvanBuuren 6:3c4a2afb11e5 21 PwmOut m1_speed(PTA5);
BasvanBuuren 0:8af618dc161b 22 DigitalOut m1_dir(PTA4);
BasvanBuuren 0:8af618dc161b 23
BasvanBuuren 5:daf561abb67d 24 //define functions
BasvanBuuren 13:b35f3553f210 25 void slam(int emg_value);
BasvanBuuren 5:daf561abb67d 26 void clamp(float * in, float min, float max);
BasvanBuuren 5:daf561abb67d 27 float pid(float rev_value, float mea_value);
BasvanBuuren 5:daf561abb67d 28
BasvanBuuren 5:daf561abb67d 29 //define global variables
BasvanBuuren 13:b35f3553f210 30 //int y1;
BasvanBuuren 9:65e998f810d1 31 float new_speed;
BasvanBuuren 9:65e998f810d1 32 float max_speed;
BasvanBuuren 9:65e998f810d1 33 float enca;
BasvanBuuren 13:b35f3553f210 34 volatile bool looptimerflag;
BasvanBuuren 13:b35f3553f210 35
BasvanBuuren 13:b35f3553f210 36 void setlooptimerflag(void)
BasvanBuuren 13:b35f3553f210 37 {
BasvanBuuren 13:b35f3553f210 38 looptimerflag = true;
BasvanBuuren 13:b35f3553f210 39 }
BasvanBuuren 0:8af618dc161b 40
BasvanBuuren 5:daf561abb67d 41 int main()
BasvanBuuren 5:daf561abb67d 42 {
BasvanBuuren 14:e9ab81429271 43
BasvanBuuren 13:b35f3553f210 44 int y1;
BasvanBuuren 13:b35f3553f210 45 Ticker looptimer;
BasvanBuuren 14:e9ab81429271 46 pc.baud(9600);
BasvanBuuren 13:b35f3553f210 47 looptimer.attach(setlooptimerflag,SAMP_TIME);
BasvanBuuren 5:daf561abb67d 48 while(1)
BasvanBuuren 14:e9ab81429271 49 {
BasvanBuuren 14:e9ab81429271 50 if(! pc.readable())
BasvanBuuren 14:e9ab81429271 51 {
BasvanBuuren 14:e9ab81429271 52 pc.printf("Main %d\n\r", encoderA.getPosition());
BasvanBuuren 14:e9ab81429271 53 wait(0.1);
BasvanBuuren 14:e9ab81429271 54 }
BasvanBuuren 14:e9ab81429271 55 else
BasvanBuuren 14:e9ab81429271 56 {
BasvanBuuren 14:e9ab81429271 57 y1 = pc.getc()-48;
BasvanBuuren 14:e9ab81429271 58 pc.printf("%d\n\r", y1 );
BasvanBuuren 14:e9ab81429271 59 slam(y1);
BasvanBuuren 14:e9ab81429271 60 }
BasvanBuuren 5:daf561abb67d 61 }
BasvanBuuren 5:daf561abb67d 62 }
BasvanBuuren 5:daf561abb67d 63
BasvanBuuren 13:b35f3553f210 64 void slam(int emg_value)
BasvanBuuren 0:8af618dc161b 65 {
BasvanBuuren 14:e9ab81429271 66 //pc.printf( "Ik begin met de slafunctie\n\r");
BasvanBuuren 14:e9ab81429271 67 //pc.printf( encoderA.getPosition() << endl << "check\n\r");
BasvanBuuren 14:e9ab81429271 68 pc.printf("Sla\n\r%d\n\r", encoderA.getPosition());
BasvanBuuren 13:b35f3553f210 69 switch (emg_value)
BasvanBuuren 6:3c4a2afb11e5 70 {
BasvanBuuren 6:3c4a2afb11e5 71 case 1:
BasvanBuuren 14:e9ab81429271 72 pc.printf( "zaak1\n\r");
BasvanBuuren 6:3c4a2afb11e5 73 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 74 max_speed=0.33;
BasvanBuuren 12:5ea8de059fa1 75 //m1_speed.write(0.33);
BasvanBuuren 14:e9ab81429271 76 //pc.printf( "Motor 1 draait\n\r");
BasvanBuuren 7:36a7a7136be3 77 break;
BasvanBuuren 6:3c4a2afb11e5 78 case 2:
BasvanBuuren 14:e9ab81429271 79 pc.printf( "zaak2\n\r");
BasvanBuuren 6:3c4a2afb11e5 80 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 81 max_speed=0.66;
BasvanBuuren 12:5ea8de059fa1 82 //m1_speed.write(0.66);
BasvanBuuren 14:e9ab81429271 83 pc.printf( "Motor 1 draait\n\r");
BasvanBuuren 7:36a7a7136be3 84 break;
BasvanBuuren 6:3c4a2afb11e5 85 case 3:
BasvanBuuren 14:e9ab81429271 86 pc.printf( "zaak3\n\r");
BasvanBuuren 6:3c4a2afb11e5 87 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 88 max_speed=1;
BasvanBuuren 12:5ea8de059fa1 89 //m1_speed.write(1);
BasvanBuuren 14:e9ab81429271 90 pc.printf( "Motor 1 draait\n\r");
BasvanBuuren 7:36a7a7136be3 91 break;
BasvanBuuren 6:3c4a2afb11e5 92 default:
BasvanBuuren 6:3c4a2afb11e5 93 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 94 m1_speed=0;
BasvanBuuren 7:36a7a7136be3 95 break;
BasvanBuuren 6:3c4a2afb11e5 96 }
BasvanBuuren 14:e9ab81429271 97 //pc.printf( "Ik ben uit de switch\n\r");
BasvanBuuren 13:b35f3553f210 98 while(!looptimerflag);
BasvanBuuren 13:b35f3553f210 99 looptimerflag=false;
BasvanBuuren 11:a9f4f296d7ed 100 new_speed=pid(MAXENCO,encoderA.getPosition());
BasvanBuuren 12:5ea8de059fa1 101 clamp(&new_speed,-max_speed,max_speed);
BasvanBuuren 12:5ea8de059fa1 102 if (new_speed>0)
BasvanBuuren 12:5ea8de059fa1 103 {
BasvanBuuren 12:5ea8de059fa1 104 m1_dir=1;
BasvanBuuren 12:5ea8de059fa1 105 }
BasvanBuuren 13:b35f3553f210 106 else if (new_speed<0)
BasvanBuuren 6:3c4a2afb11e5 107 {
BasvanBuuren 12:5ea8de059fa1 108 m1_dir=0;
BasvanBuuren 12:5ea8de059fa1 109 }
BasvanBuuren 12:5ea8de059fa1 110 m1_speed.write(fabs(new_speed));
BasvanBuuren 12:5ea8de059fa1 111
BasvanBuuren 12:5ea8de059fa1 112 while(new_speed<-0.1 || new_speed>0.1)
BasvanBuuren 12:5ea8de059fa1 113 {
BasvanBuuren 13:b35f3553f210 114 while(!looptimerflag);
BasvanBuuren 13:b35f3553f210 115 looptimerflag=false;
BasvanBuuren 14:e9ab81429271 116 //pc.printf( "Eat, Sleep, Rave, Repeat\n\r");
BasvanBuuren 6:3c4a2afb11e5 117 new_speed=pid(MAXENCO,encoderA.getPosition());
BasvanBuuren 6:3c4a2afb11e5 118 clamp(&new_speed,-max_speed,max_speed);
BasvanBuuren 6:3c4a2afb11e5 119 if (new_speed>0)
BasvanBuuren 6:3c4a2afb11e5 120 {
BasvanBuuren 6:3c4a2afb11e5 121 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 122 }
BasvanBuuren 13:b35f3553f210 123 else if (new_speed<0)
BasvanBuuren 6:3c4a2afb11e5 124 {
BasvanBuuren 6:3c4a2afb11e5 125 m1_dir=0;
BasvanBuuren 6:3c4a2afb11e5 126 }
BasvanBuuren 6:3c4a2afb11e5 127 m1_speed.write(fabs(new_speed));
BasvanBuuren 14:e9ab81429271 128 //pc.printf( new_speed << endl;
BasvanBuuren 6:3c4a2afb11e5 129 }
BasvanBuuren 14:e9ab81429271 130 pc.printf( "Ik ben uit de eerste whilelus\n\r");
BasvanBuuren 14:e9ab81429271 131 //pc.printf( encoderA.getPosition() << endl << "check\n\r");
BasvanBuuren 14:e9ab81429271 132 pc.printf("%d\n\r", encoderA.getPosition());
BasvanBuuren 13:b35f3553f210 133 while(!looptimerflag);
BasvanBuuren 13:b35f3553f210 134 looptimerflag=false;
BasvanBuuren 11:a9f4f296d7ed 135 new_speed=pid(MINENCO,encoderA.getPosition());
BasvanBuuren 12:5ea8de059fa1 136 clamp(&new_speed,-max_speed,max_speed);
BasvanBuuren 12:5ea8de059fa1 137 if (new_speed>0)
BasvanBuuren 12:5ea8de059fa1 138 {
BasvanBuuren 12:5ea8de059fa1 139 m1_dir=1;
BasvanBuuren 12:5ea8de059fa1 140 }
BasvanBuuren 13:b35f3553f210 141 else if (new_speed<0)
BasvanBuuren 12:5ea8de059fa1 142 {
BasvanBuuren 12:5ea8de059fa1 143 m1_dir=0;
BasvanBuuren 12:5ea8de059fa1 144 }
BasvanBuuren 12:5ea8de059fa1 145 m1_speed.write(fabs(new_speed));
BasvanBuuren 12:5ea8de059fa1 146
BasvanBuuren 12:5ea8de059fa1 147 while(new_speed<-0.1 || new_speed>0.1)
BasvanBuuren 6:3c4a2afb11e5 148 {
BasvanBuuren 13:b35f3553f210 149 while(!looptimerflag);
BasvanBuuren 13:b35f3553f210 150 looptimerflag=false;
BasvanBuuren 6:3c4a2afb11e5 151 new_speed=pid(MINENCO,encoderA.getPosition());
BasvanBuuren 6:3c4a2afb11e5 152 clamp(&new_speed,-0.5,0.5);
BasvanBuuren 6:3c4a2afb11e5 153 if (new_speed>0)
BasvanBuuren 6:3c4a2afb11e5 154 {
BasvanBuuren 6:3c4a2afb11e5 155 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 156 }
BasvanBuuren 13:b35f3553f210 157 else if (new_speed<0)
BasvanBuuren 6:3c4a2afb11e5 158 {
BasvanBuuren 6:3c4a2afb11e5 159 m1_dir=0;
BasvanBuuren 6:3c4a2afb11e5 160 }
BasvanBuuren 6:3c4a2afb11e5 161 m1_speed.write(fabs(new_speed));
BasvanBuuren 6:3c4a2afb11e5 162 }
BasvanBuuren 12:5ea8de059fa1 163 m1_speed.write(0);
BasvanBuuren 14:e9ab81429271 164 pc.printf( "Ik ben uit de tweede whilelus\n\r");
BasvanBuuren 14:e9ab81429271 165 //pc.printf( encoderA.getPosition() << endl << "check\n\r");
BasvanBuuren 14:e9ab81429271 166 pc.printf("%d\n\r", encoderA.getPosition());
BasvanBuuren 0:8af618dc161b 167 }
BasvanBuuren 0:8af618dc161b 168
BasvanBuuren 5:daf561abb67d 169 void clamp(float * in, float min, float max)
BasvanBuuren 5:daf561abb67d 170 {
BasvanBuuren 5:daf561abb67d 171 *in > min ? * in < max? : *in = max : *in = min;
BasvanBuuren 5:daf561abb67d 172 }
BasvanBuuren 5:daf561abb67d 173
BasvanBuuren 5:daf561abb67d 174 float pid(float rev_value, float mea_value)
BasvanBuuren 0:8af618dc161b 175 {
BasvanBuuren 5:daf561abb67d 176 float error;
BasvanBuuren 5:daf561abb67d 177 static float prev_error = 0;
BasvanBuuren 5:daf561abb67d 178 float p_out = 0;
BasvanBuuren 5:daf561abb67d 179 static float i_out = 0;
BasvanBuuren 5:daf561abb67d 180 float d_out = 0;
BasvanBuuren 5:daf561abb67d 181 error = rev_value - mea_value;
BasvanBuuren 5:daf561abb67d 182 p_out = error * K_P;
BasvanBuuren 5:daf561abb67d 183 i_out += error * K_I;
BasvanBuuren 5:daf561abb67d 184 d_out = (error - prev_error) * K_D;
BasvanBuuren 5:daf561abb67d 185 clamp(&i_out,-I_LIMIT,I_LIMIT);
BasvanBuuren 5:daf561abb67d 186 prev_error=error;
BasvanBuuren 5:daf561abb67d 187 return p_out + i_out + d_out;
BasvanBuuren 0:8af618dc161b 188 }