Thomas Burgers / Mbed 2 deprecated Samenwerking_Motor12

Samenwerking Groep 12

Dependencies:   Encoder MODSERIAL HIDScope mbed

Foo

main.cpp@25:75cce8f86272, 2015-09-22 (annotated)

Committer:
ThomasBNL
Date:
Tue Sep 22 21:36:28 2015 +0000
Revision:
25:75cce8f86272
Parent:
24:465efbd884d5
Child:
26:6bdfc1856531
Created structure with comments and lay-out; -> Basic P-controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 25:75cce8f86272 1 //------------------------------------------------------------------------------------------------------------------------------------
ThomasBNL 25:75cce8f86272 2 // GROUP 12: THE CHENNE ROBOT (bottle xylophone)
ThomasBNL 25:75cce8f86272 3 //------------------------------------------------------------------------------------------------------------------------------------
ThomasBNL 25:75cce8f86272 4 //INCLUDE LIBRARY
ThomasBNL 25:75cce8f86272 5 //------------------------------------------------------------------------------------------------------------------------------------
ThomasBNL 10:e923f51f18c4 6 #include "mbed.h"
ThomasBNL 10:e923f51f18c4 7 #include "HIDScope.h"
ThomasBNL 10:e923f51f18c4 8 #include "encoder.h"
ThomasBNL 10:e923f51f18c4 9 #include "MODSERIAL.h"
ThomasBNL 10:e923f51f18c4 10
ThomasBNL 25:75cce8f86272 11 //------------------------------------------------------------------------------------------------------------------------------------
ThomasBNL 25:75cce8f86272 12 // Defining inputs/outputs/constants
ThomasBNL 25:75cce8f86272 13 //------------------------------------------------------------------------------------------------------------------------------------
ThomasBNL 25:75cce8f86272 14
ThomasBNL 25:75cce8f86272 15 // MOTOR 2 SIGNALS
ThomasBNL 25:75cce8f86272 16 DigitalOut motor2direction(D4); //D4 and D5 are Motor 2 inputs
ThomasBNL 25:75cce8f86272 17 PwmOut motor2speed(D5); // D5 is an input on the motor that controls the speed of motor number 2
ThomasBNL 25:75cce8f86272 18 DigitalIn button(PTA4); // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)
ThomasBNL 25:75cce8f86272 19 Encoder motor2(D13,D12,true); // The encoder is able to read and set position/speed values
ThomasBNL 25:75cce8f86272 20 Ticker TickerController; // This adds a Ticker to the function +TickerController
ThomasBNL 25:75cce8f86272 21 //AnalogIn potmeter2(A0); // NOT IMPLEMENTED YET
ThomasBNL 25:75cce8f86272 22
ThomasBNL 25:75cce8f86272 23
ThomasBNL 25:75cce8f86272 24 // OUTPUT DATA
ThomasBNL 25:75cce8f86272 25 MODSERIAL pc(USBTX,USBRX); // This input is used to send data to the pc
ThomasBNL 25:75cce8f86272 26 HIDScope scope(2); // This adds two HIDScope channels (1) and (0) to display signals to HIDScope
ThomasBNL 25:75cce8f86272 27
ThomasBNL 13:a0113cf2fc5f 28
ThomasBNL 25:75cce8f86272 29 // DEFINING CONSTANTS
ThomasBNL 25:75cce8f86272 30 double counts_per_rev=4200; // Counts per revolution DERIVED FROM TESTS (NOG DOEN)
ThomasBNL 25:75cce8f86272 31 double degrees_per_turn=360; // Degrees for one revolution
ThomasBNL 25:75cce8f86272 32 double counts_per_degree=counts_per_rev/degrees_per_turn; // 11.67 counts/degree //divide counts by 11.67 to obtain degrees
ThomasBNL 25:75cce8f86272 33 const double motor2_Kp = 1; // Controller gain which will be multiplied with the error (*how fast will the error be corrected)
ThomasBNL 25:75cce8f86272 34 const double reference = 400; // Reference value to what position (counts) shall the motor return
ThomasBNL 13:a0113cf2fc5f 35
ThomasBNL 25:75cce8f86272 36 //------------------------------------------------------------------------------------------------------------------------------------
ThomasBNL 25:75cce8f86272 37 // Defining functions
ThomasBNL 25:75cce8f86272 38 //------------------------------------------------------------------------------------------------------------------------------------
ThomasBNL 25:75cce8f86272 39
ThomasBNL 25:75cce8f86272 40 // FUNCTION 1: P-controller
ThomasBNL 19:269f19917d80 41 double P(double error, const double Kp) //returns error * controller gain
ThomasBNL 25:75cce8f86272 42 {
ThomasBNL 25:75cce8f86272 43 return Kp*error;
ThomasBNL 25:75cce8f86272 44 }
ThomasBNL 25:75cce8f86272 45
ThomasBNL 25:75cce8f86272 46
ThomasBNL 25:75cce8f86272 47 // FUNCTION 2: Calculates the errors and puts the value into function 1
ThomasBNL 25:75cce8f86272 48 double motor2_controller()
ThomasBNL 19:269f19917d80 49 {
ThomasBNL 19:269f19917d80 50 double position=motor2.getPosition(); //current motor position
ThomasBNL 21:fb17e0b8924e 51 double error=reference-position;
ThomasBNL 24:465efbd884d5 52 scope.set(0,motor2.getPosition());
ThomasBNL 24:465efbd884d5 53 scope.send();
ThomasBNL 21:fb17e0b8924e 54 pc.printf("ik doe het positie = %d en error =%d en reference=%d \r\n", position, error, reference);
ThomasBNL 21:fb17e0b8924e 55 return P(error,motor2_Kp);
ThomasBNL 20:4d128c3f1228 56 }
ThomasBNL 13:a0113cf2fc5f 57
ThomasBNL 25:75cce8f86272 58 //------------------------------------------------------------------------------------------------------------------------------------
ThomasBNL 25:75cce8f86272 59 // Execution of the programm (MAIN)
ThomasBNL 25:75cce8f86272 60 //------------------------------------------------------------------------------------------------------------------------------------
ThomasBNL 25:75cce8f86272 61
ThomasBNL 19:269f19917d80 62 // MAIN
ThomasBNL 25:75cce8f86272 63 int main() // Begin main loop 1
ThomasBNL 10:e923f51f18c4 64 {
ThomasBNL 25:75cce8f86272 65 // CALLIBRATION BEGIN POSITION
ThomasBNL 25:75cce8f86272 66 pc.baud(9600); // baud rate at which the information is processed to the computer
ThomasBNL 25:75cce8f86272 67 //motor2.setPosition(0); // calls the current position of the motor zero
ThomasBNL 21:fb17e0b8924e 68
ThomasBNL 25:75cce8f86272 69 while(true) { // while loop 1
ThomasBNL 25:75cce8f86272 70 // IF BUTTON IS PRESSED
ThomasBNL 25:75cce8f86272 71 if (button.read() < 0.5)
ThomasBNL 25:75cce8f86272 72 { // start if else loop 1
ThomasBNL 25:75cce8f86272 73 motor2direction = 0; // clockwise motor rotation (front view)
ThomasBNL 25:75cce8f86272 74 motor2speed = 0.5f; // motorspeed
ThomasBNL 25:75cce8f86272 75
ThomasBNL 25:75cce8f86272 76 pc.printf("positie = %d \r\n", motor2.getPosition());
ThomasBNL 25:75cce8f86272 77 }
ThomasBNL 25:75cce8f86272 78 // IF BUTTON IS NOT PRESSED
ThomasBNL 25:75cce8f86272 79 else
ThomasBNL 25:75cce8f86272 80 {
ThomasBNL 25:75cce8f86272 81 double output_motor = motor2_controller(); //extract value from motor2_controller
ThomasBNL 25:75cce8f86272 82
ThomasBNL 25:75cce8f86272 83 //Case 1
ThomasBNL 25:75cce8f86272 84 while(output_motor>0) // Value to correct for by the motor >0
ThomasBNL 25:75cce8f86272 85 {
ThomasBNL 25:75cce8f86272 86 motor2speed=output_motor; //then apply this value
ThomasBNL 25:75cce8f86272 87 motor2direction=1; //to the motor in anti-clockwise direction
ThomasBNL 25:75cce8f86272 88 break; // abort the loop and continue the script
ThomasBNL 25:75cce8f86272 89 }
ThomasBNL 25:75cce8f86272 90
ThomasBNL 25:75cce8f86272 91 //Case 2
ThomasBNL 25:75cce8f86272 92 while(output_motor<0) // Value to correct for by the motor <0
ThomasBNL 25:75cce8f86272 93 {
ThomasBNL 25:75cce8f86272 94 motor2speed=fabs(output_motor);// then apply the absolute value
ThomasBNL 25:75cce8f86272 95 motor2direction=0; // to the motor in clockwise direction
ThomasBNL 25:75cce8f86272 96 break; // then abort the loop and continue the script
ThomasBNL 25:75cce8f86272 97 }
ThomasBNL 25:75cce8f86272 98 // ATTACHING TICKER
ThomasBNL 25:75cce8f86272 99 {
ThomasBNL 25:75cce8f86272 100 TickerController.attach(&motor2_controller,0.01f);//100Hz
ThomasBNL 25:75cce8f86272 101 pc.printf("else loop controller");
ThomasBNL 25:75cce8f86272 102 }
ThomasBNL 25:75cce8f86272 103 } // end of if-else loop 1
ThomasBNL 25:75cce8f86272 104
ThomasBNL 25:75cce8f86272 105 // CALLIBRATION POSITION OF MOTOR
ThomasBNL 25:75cce8f86272 106 // The position is limited to number of counts per revolution
ThomasBNL 25:75cce8f86272 107
ThomasBNL 25:75cce8f86272 108 while ((motor2.getPosition()>counts_per_rev) || (motor2.getPosition()<-counts_per_rev))
ThomasBNL 25:75cce8f86272 109 // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
ThomasBNL 25:75cce8f86272 110 {
ThomasBNL 25:75cce8f86272 111 motor2.setPosition(0);
ThomasBNL 25:75cce8f86272 112 pc.printf(" HE \r\n LLO \r\n WO \r\n RLD \r\n !!! \r\n FOO! \r\n");
ThomasBNL 25:75cce8f86272 113 break;
ThomasBNL 25:75cce8f86272 114 }
ThomasBNL 25:75cce8f86272 115 } // end of while loop 1
ThomasBNL 25:75cce8f86272 116 } // end of main
ThomasBNL 25:75cce8f86272 117
ThomasBNL 25:75cce8f86272 118 //------------------------------------------------------------------------------------------------------------------------------------