jordy morsinkhof / Mbed 2 deprecated Biorobotics_Motor_Control

RobotArm_ Biorobotics project.

Dependencies:   PID QEI RemoteIR Servo mbed

Fork of Biorobotics_Motor_Control by Bram S

main.cpp@2:684d5cf2f683, 2017-10-23 (annotated)

Committer:
jordymorsinkhof
Date:
Mon Oct 23 10:57:11 2017 +0000
Revision:
2:684d5cf2f683
Parent:
0:6c0f87177797
Child:
3:98ea6afa0cf2
Biorobotics_Motor_Control; ; *Homing; *Remote Controller; *

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BramS23 0:6c0f87177797 1 #include "mbed.h"
BramS23 0:6c0f87177797 2 #include "QEI.h"
jordymorsinkhof 2:684d5cf2f683 3 #include "PID.h"
jordymorsinkhof 2:684d5cf2f683 4 #include "ReceiverIR.h"
jordymorsinkhof 2:684d5cf2f683 5 #include "Servo.h"
jordymorsinkhof 2:684d5cf2f683 6
jordymorsinkhof 2:684d5cf2f683 7 ReceiverIR ir_rx(D2);
jordymorsinkhof 2:684d5cf2f683 8
jordymorsinkhof 2:684d5cf2f683 9 AnalogIn PotMeter1(A0);
jordymorsinkhof 2:684d5cf2f683 10 AnalogIn PotMeter2(A1);
BramS23 0:6c0f87177797 11 InterruptIn Button(PTA4);
BramS23 0:6c0f87177797 12
BramS23 0:6c0f87177797 13 PwmOut MotorThrottle1(D5);
BramS23 0:6c0f87177797 14 PwmOut MotorThrottle2(D6);
BramS23 0:6c0f87177797 15 DigitalOut MotorDirection1(D4);
BramS23 0:6c0f87177797 16 DigitalOut MotorDirection2(D7);
jordymorsinkhof 2:684d5cf2f683 17 DigitalOut servo(D3);
BramS23 0:6c0f87177797 18
jordymorsinkhof 2:684d5cf2f683 19 DigitalIn EndSwitch1(D9);
jordymorsinkhof 2:684d5cf2f683 20 DigitalIn EndSwitch2(D8);
jordymorsinkhof 2:684d5cf2f683 21
jordymorsinkhof 2:684d5cf2f683 22 //DigitalOut Magneet(D7);
jordymorsinkhof 2:684d5cf2f683 23
jordymorsinkhof 2:684d5cf2f683 24
jordymorsinkhof 2:684d5cf2f683 25 QEI EncoderMotor1(D12,D13,NC,4200);
jordymorsinkhof 2:684d5cf2f683 26 QEI EncoderMotor2(D10,D11,NC,4200);
jordymorsinkhof 2:684d5cf2f683 27
jordymorsinkhof 2:684d5cf2f683 28 Ticker ControlTicker;
jordymorsinkhof 2:684d5cf2f683 29 Ticker GetRefTicker;
jordymorsinkhof 2:684d5cf2f683 30
jordymorsinkhof 2:684d5cf2f683 31 float Interval=0.02f;
jordymorsinkhof 2:684d5cf2f683 32 float pi=3.14159268;
jordymorsinkhof 2:684d5cf2f683 33 PID controller1(20.0f,0.0f,0.001f, Interval);
jordymorsinkhof 2:684d5cf2f683 34 PID controller2(20.0f,0.0f,0.002f, Interval);
jordymorsinkhof 2:684d5cf2f683 35
jordymorsinkhof 2:684d5cf2f683 36 RemoteIR::Format format;
jordymorsinkhof 2:684d5cf2f683 37 uint8_t buf[4];
BramS23 0:6c0f87177797 38
BramS23 0:6c0f87177797 39 Serial pc(PTB17,PTB16);
BramS23 0:6c0f87177797 40
jordymorsinkhof 2:684d5cf2f683 41 float PosRef1=1.0f;
jordymorsinkhof 2:684d5cf2f683 42 float PosRef2=1.0f;
jordymorsinkhof 2:684d5cf2f683 43
jordymorsinkhof 2:684d5cf2f683 44
jordymorsinkhof 2:684d5cf2f683 45
jordymorsinkhof 2:684d5cf2f683 46
jordymorsinkhof 2:684d5cf2f683 47 void HomingLoop(){
jordymorsinkhof 2:684d5cf2f683 48
jordymorsinkhof 2:684d5cf2f683 49 EndSwitch1.mode(PullUp);
jordymorsinkhof 2:684d5cf2f683 50 EndSwitch2.mode(PullUp);
jordymorsinkhof 2:684d5cf2f683 51
jordymorsinkhof 2:684d5cf2f683 52 MotorThrottle1=0.1f;
jordymorsinkhof 2:684d5cf2f683 53 MotorThrottle2=0.2f;
jordymorsinkhof 2:684d5cf2f683 54
jordymorsinkhof 2:684d5cf2f683 55 MotorDirection1=1;
jordymorsinkhof 2:684d5cf2f683 56 MotorDirection2=1;
jordymorsinkhof 2:684d5cf2f683 57
jordymorsinkhof 2:684d5cf2f683 58 bool dummy1=0;
jordymorsinkhof 2:684d5cf2f683 59 bool dummy2=0;
jordymorsinkhof 2:684d5cf2f683 60
jordymorsinkhof 2:684d5cf2f683 61 while(EndSwitch1.read()|EndSwitch2.read()){
jordymorsinkhof 2:684d5cf2f683 62 if((EndSwitch1.read()==0)&&(dummy1==0)){
jordymorsinkhof 2:684d5cf2f683 63 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 64 dummy1=1;
jordymorsinkhof 2:684d5cf2f683 65 }
jordymorsinkhof 2:684d5cf2f683 66 if((EndSwitch2.read()==0)&&(dummy2==0)){
jordymorsinkhof 2:684d5cf2f683 67 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 68 dummy2=1;
jordymorsinkhof 2:684d5cf2f683 69 }
jordymorsinkhof 2:684d5cf2f683 70 }
jordymorsinkhof 2:684d5cf2f683 71 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 72 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 73 EncoderMotor1.reset();
jordymorsinkhof 2:684d5cf2f683 74 EncoderMotor2.reset();
jordymorsinkhof 2:684d5cf2f683 75 }
jordymorsinkhof 2:684d5cf2f683 76
jordymorsinkhof 2:684d5cf2f683 77 void ControlLoop(){
jordymorsinkhof 2:684d5cf2f683 78
jordymorsinkhof 2:684d5cf2f683 79 float Pos1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
jordymorsinkhof 2:684d5cf2f683 80 float Pos2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
jordymorsinkhof 2:684d5cf2f683 81 float error1 = PosRef1 - Pos1;
jordymorsinkhof 2:684d5cf2f683 82 float error2 = PosRef2 - Pos2;
jordymorsinkhof 2:684d5cf2f683 83
jordymorsinkhof 2:684d5cf2f683 84 // PID input
jordymorsinkhof 2:684d5cf2f683 85 controller1.setSetPoint(PosRef1);
jordymorsinkhof 2:684d5cf2f683 86 controller2.setSetPoint(PosRef2);
jordymorsinkhof 2:684d5cf2f683 87
jordymorsinkhof 2:684d5cf2f683 88 controller1.setProcessValue(Pos1);
jordymorsinkhof 2:684d5cf2f683 89 controller2.setProcessValue(Pos2);
jordymorsinkhof 2:684d5cf2f683 90
jordymorsinkhof 2:684d5cf2f683 91 // PID output
jordymorsinkhof 2:684d5cf2f683 92 float OutPut1=controller1.compute();
jordymorsinkhof 2:684d5cf2f683 93 float OutPut2=controller2.compute();
jordymorsinkhof 2:684d5cf2f683 94
jordymorsinkhof 2:684d5cf2f683 95 // Direction handling
jordymorsinkhof 2:684d5cf2f683 96 float Direction1=0;
jordymorsinkhof 2:684d5cf2f683 97 float Direction2=0;
jordymorsinkhof 2:684d5cf2f683 98
jordymorsinkhof 2:684d5cf2f683 99 if(OutPut1<0){
jordymorsinkhof 2:684d5cf2f683 100 Direction1=1;
jordymorsinkhof 2:684d5cf2f683 101 }
jordymorsinkhof 2:684d5cf2f683 102 if(OutPut2<0){
jordymorsinkhof 2:684d5cf2f683 103 Direction2=1;
jordymorsinkhof 2:684d5cf2f683 104 }
jordymorsinkhof 2:684d5cf2f683 105
jordymorsinkhof 2:684d5cf2f683 106 OutPut1=fabs(OutPut1);
jordymorsinkhof 2:684d5cf2f683 107 OutPut2=fabs(OutPut2);
jordymorsinkhof 2:684d5cf2f683 108
jordymorsinkhof 2:684d5cf2f683 109 // Output schrijven
jordymorsinkhof 2:684d5cf2f683 110 MotorThrottle1.write(OutPut1);
jordymorsinkhof 2:684d5cf2f683 111 MotorThrottle2.write(OutPut2);
jordymorsinkhof 2:684d5cf2f683 112 MotorDirection1=Direction1;
jordymorsinkhof 2:684d5cf2f683 113 MotorDirection2=Direction2;
BramS23 0:6c0f87177797 114 }
BramS23 0:6c0f87177797 115
BramS23 0:6c0f87177797 116
jordymorsinkhof 2:684d5cf2f683 117 void PickUp(){
jordymorsinkhof 2:684d5cf2f683 118
jordymorsinkhof 2:684d5cf2f683 119 }
jordymorsinkhof 2:684d5cf2f683 120
jordymorsinkhof 2:684d5cf2f683 121 void LayDown(){
jordymorsinkhof 2:684d5cf2f683 122
jordymorsinkhof 2:684d5cf2f683 123
jordymorsinkhof 2:684d5cf2f683 124 }
jordymorsinkhof 2:684d5cf2f683 125
jordymorsinkhof 2:684d5cf2f683 126
jordymorsinkhof 2:684d5cf2f683 127 void RemoteController(){
jordymorsinkhof 2:684d5cf2f683 128
jordymorsinkhof 2:684d5cf2f683 129 int bitcount;
jordymorsinkhof 2:684d5cf2f683 130
jordymorsinkhof 2:684d5cf2f683 131
jordymorsinkhof 2:684d5cf2f683 132 bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
jordymorsinkhof 2:684d5cf2f683 133 int state = buf[2];
jordymorsinkhof 2:684d5cf2f683 134 switch(state)
jordymorsinkhof 2:684d5cf2f683 135 {
jordymorsinkhof 2:684d5cf2f683 136 case 22: //1
jordymorsinkhof 2:684d5cf2f683 137 printf("1\n\r");
jordymorsinkhof 2:684d5cf2f683 138 break;
jordymorsinkhof 2:684d5cf2f683 139 case 25: //1
jordymorsinkhof 2:684d5cf2f683 140 printf("2\n\r");
jordymorsinkhof 2:684d5cf2f683 141 break;
jordymorsinkhof 2:684d5cf2f683 142 case 13: //1
jordymorsinkhof 2:684d5cf2f683 143 printf("3\n\r");
jordymorsinkhof 2:684d5cf2f683 144 break;
jordymorsinkhof 2:684d5cf2f683 145 case 12: //1
jordymorsinkhof 2:684d5cf2f683 146 printf("4\n\r");
jordymorsinkhof 2:684d5cf2f683 147 break;
jordymorsinkhof 2:684d5cf2f683 148 case 24: //1
jordymorsinkhof 2:684d5cf2f683 149 printf("5\n\r");
jordymorsinkhof 2:684d5cf2f683 150 ControlTicker.detach();
jordymorsinkhof 2:684d5cf2f683 151 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 152 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 153 PickUp();
jordymorsinkhof 2:684d5cf2f683 154 ControlTicker.attach(&ControlLoop, Interval);
jordymorsinkhof 2:684d5cf2f683 155 break;
jordymorsinkhof 2:684d5cf2f683 156 case 94: //1
jordymorsinkhof 2:684d5cf2f683 157 printf("6\n\r");
jordymorsinkhof 2:684d5cf2f683 158 break;
jordymorsinkhof 2:684d5cf2f683 159 case 8: //1
jordymorsinkhof 2:684d5cf2f683 160 printf("7\n\r");
jordymorsinkhof 2:684d5cf2f683 161 break;
jordymorsinkhof 2:684d5cf2f683 162 case 28: //1
jordymorsinkhof 2:684d5cf2f683 163 printf("8\n\r");
jordymorsinkhof 2:684d5cf2f683 164 ControlTicker.detach();
jordymorsinkhof 2:684d5cf2f683 165 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 166 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 167 LayDown();
jordymorsinkhof 2:684d5cf2f683 168 ControlTicker.attach(&ControlLoop, Interval);
jordymorsinkhof 2:684d5cf2f683 169 break;
jordymorsinkhof 2:684d5cf2f683 170 case 90: //1
jordymorsinkhof 2:684d5cf2f683 171 printf("9\n\r");
jordymorsinkhof 2:684d5cf2f683 172 break;
jordymorsinkhof 2:684d5cf2f683 173 case 70: //1
jordymorsinkhof 2:684d5cf2f683 174 pc.printf("Boven\n\r");
jordymorsinkhof 2:684d5cf2f683 175 PosRef2=PosRef2-0.1f;
jordymorsinkhof 2:684d5cf2f683 176 pc.printf("%f\n\r", PosRef2);
jordymorsinkhof 2:684d5cf2f683 177 break;
jordymorsinkhof 2:684d5cf2f683 178 case 21: //1
jordymorsinkhof 2:684d5cf2f683 179 pc.printf("Onder\n\r");
jordymorsinkhof 2:684d5cf2f683 180 PosRef2=PosRef2+0.1f;
jordymorsinkhof 2:684d5cf2f683 181 pc.printf("%f\n\r", PosRef2);
jordymorsinkhof 2:684d5cf2f683 182 break;
jordymorsinkhof 2:684d5cf2f683 183 case 68: //1
jordymorsinkhof 2:684d5cf2f683 184 pc.printf("Links\n\r");
jordymorsinkhof 2:684d5cf2f683 185 PosRef1=PosRef1+0.1f;
jordymorsinkhof 2:684d5cf2f683 186 pc.printf("%f\n\r", PosRef1);
jordymorsinkhof 2:684d5cf2f683 187 break;
jordymorsinkhof 2:684d5cf2f683 188 case 67: //1
jordymorsinkhof 2:684d5cf2f683 189 pc.printf("Rechts\n\r");
jordymorsinkhof 2:684d5cf2f683 190 PosRef1=PosRef1-0.1f;
jordymorsinkhof 2:684d5cf2f683 191 pc.printf("%f\n\r", PosRef1);
jordymorsinkhof 2:684d5cf2f683 192 break;
jordymorsinkhof 2:684d5cf2f683 193 case 64: //1
jordymorsinkhof 2:684d5cf2f683 194 pc.printf("OK\n\r");
jordymorsinkhof 2:684d5cf2f683 195 ControlTicker.detach();
jordymorsinkhof 2:684d5cf2f683 196 MotorThrottle1=0.0f;
jordymorsinkhof 2:684d5cf2f683 197 MotorThrottle2=0.0f;
jordymorsinkhof 2:684d5cf2f683 198 HomingLoop();
jordymorsinkhof 2:684d5cf2f683 199 ControlTicker.attach(&ControlLoop, Interval);
jordymorsinkhof 2:684d5cf2f683 200
jordymorsinkhof 2:684d5cf2f683 201 break;
jordymorsinkhof 2:684d5cf2f683 202 default:
jordymorsinkhof 2:684d5cf2f683 203 break;
jordymorsinkhof 2:684d5cf2f683 204 }
jordymorsinkhof 2:684d5cf2f683 205 }
jordymorsinkhof 2:684d5cf2f683 206
jordymorsinkhof 2:684d5cf2f683 207
jordymorsinkhof 2:684d5cf2f683 208
jordymorsinkhof 2:684d5cf2f683 209
jordymorsinkhof 2:684d5cf2f683 210 void DeterminePosRef(){
jordymorsinkhof 2:684d5cf2f683 211 PosRef1=2*pi*PotMeter1.read(); // Reference in Rad
jordymorsinkhof 2:684d5cf2f683 212 PosRef2=2*pi*PotMeter2.read(); // Reference in Rad
jordymorsinkhof 2:684d5cf2f683 213 }
jordymorsinkhof 2:684d5cf2f683 214
BramS23 0:6c0f87177797 215 int main() {
BramS23 0:6c0f87177797 216 pc.baud(115200);
BramS23 0:6c0f87177797 217 pc.printf("startup");
jordymorsinkhof 2:684d5cf2f683 218
jordymorsinkhof 2:684d5cf2f683 219 controller1.setInputLimits(0.5f,2.0f*pi);
jordymorsinkhof 2:684d5cf2f683 220 controller2.setInputLimits(0.5f,2.0f*pi);
jordymorsinkhof 2:684d5cf2f683 221 controller1.setOutputLimits(-0.15f,0.15f);
jordymorsinkhof 2:684d5cf2f683 222 controller2.setOutputLimits(-0.5f,0.5f);
jordymorsinkhof 2:684d5cf2f683 223
jordymorsinkhof 2:684d5cf2f683 224
jordymorsinkhof 2:684d5cf2f683 225 HomingLoop();
jordymorsinkhof 2:684d5cf2f683 226
BramS23 0:6c0f87177797 227
jordymorsinkhof 2:684d5cf2f683 228 ControlTicker.attach(&ControlLoop, Interval);
jordymorsinkhof 2:684d5cf2f683 229 //GetRefTicker.attach(&DeterminePosRef, 0.1f);
jordymorsinkhof 2:684d5cf2f683 230
jordymorsinkhof 2:684d5cf2f683 231
jordymorsinkhof 2:684d5cf2f683 232
jordymorsinkhof 2:684d5cf2f683 233 while(1)
jordymorsinkhof 2:684d5cf2f683 234 {
jordymorsinkhof 2:684d5cf2f683 235 if (ir_rx.getState() == ReceiverIR::Received)
jordymorsinkhof 2:684d5cf2f683 236 {
jordymorsinkhof 2:684d5cf2f683 237
jordymorsinkhof 2:684d5cf2f683 238 pc.printf("received");
jordymorsinkhof 2:684d5cf2f683 239 RemoteController();
jordymorsinkhof 2:684d5cf2f683 240 wait(0.01);
BramS23 0:6c0f87177797 241 }
jordymorsinkhof 2:684d5cf2f683 242 }
jordymorsinkhof 2:684d5cf2f683 243
jordymorsinkhof 2:684d5cf2f683 244
jordymorsinkhof 2:684d5cf2f683 245 while(1){}
jordymorsinkhof 2:684d5cf2f683 246
jordymorsinkhof 2:684d5cf2f683 247
BramS23 0:6c0f87177797 248 }