Thomas Burgers / Mbed 2 deprecated Samenwerking_Motor12

Samenwerking Groep 12

Dependencies:   Encoder MODSERIAL HIDScope mbed

Foo

main.cpp@26:6bdfc1856531, 2015-09-22 (annotated)

Committer:
ThomasBNL
Date:
Tue Sep 22 21:42:11 2015 +0000
Revision:
26:6bdfc1856531
Parent:
25:75cce8f86272 
Changed the order of the MAIN -> callibration revolutions naarboven gehaald

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 26:6bdfc1856531 63 int main() // Begin main loop 1
ThomasBNL 10:e923f51f18c4 64 {
ThomasBNL 26:6bdfc1856531 65 // CALLIBRATION STARTING POSITION
ThomasBNL 26:6bdfc1856531 66
ThomasBNL 25:75cce8f86272 67 pc.baud(9600); // baud rate at which the information is processed to the computer
ThomasBNL 25:75cce8f86272 68 //motor2.setPosition(0); // calls the current position of the motor zero
ThomasBNL 26:6bdfc1856531 69
ThomasBNL 26:6bdfc1856531 70 // CALLIBRATION POSITION OF MOTOR
ThomasBNL 26:6bdfc1856531 71 // The position is limited to number of counts per revolution
ThomasBNL 26:6bdfc1856531 72
ThomasBNL 26:6bdfc1856531 73 while ((motor2.getPosition()>counts_per_rev) || (motor2.getPosition()<-counts_per_rev))
ThomasBNL 26:6bdfc1856531 74 // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
ThomasBNL 26:6bdfc1856531 75 {
ThomasBNL 26:6bdfc1856531 76 motor2.setPosition(0);
ThomasBNL 26:6bdfc1856531 77 pc.printf(" HE \r\n LLO \r\n WO \r\n RLD \r\n !!! \r\n FOO! \r\n");
ThomasBNL 26:6bdfc1856531 78 break;
ThomasBNL 26:6bdfc1856531 79 }
ThomasBNL 21:fb17e0b8924e 80
ThomasBNL 26:6bdfc1856531 81 while(true) { // while loop 1
ThomasBNL 25:75cce8f86272 82 // IF BUTTON IS PRESSED
ThomasBNL 25:75cce8f86272 83 if (button.read() < 0.5)
ThomasBNL 26:6bdfc1856531 84 { // start if else loop 1
ThomasBNL 25:75cce8f86272 85 motor2direction = 0; // clockwise motor rotation (front view)
ThomasBNL 25:75cce8f86272 86 motor2speed = 0.5f; // motorspeed
ThomasBNL 25:75cce8f86272 87
ThomasBNL 25:75cce8f86272 88 pc.printf("positie = %d \r\n", motor2.getPosition());
ThomasBNL 25:75cce8f86272 89 }
ThomasBNL 25:75cce8f86272 90 // IF BUTTON IS NOT PRESSED
ThomasBNL 25:75cce8f86272 91 else
ThomasBNL 25:75cce8f86272 92 {
ThomasBNL 25:75cce8f86272 93 double output_motor = motor2_controller(); //extract value from motor2_controller
ThomasBNL 25:75cce8f86272 94
ThomasBNL 25:75cce8f86272 95 //Case 1
ThomasBNL 25:75cce8f86272 96 while(output_motor>0) // Value to correct for by the motor >0
ThomasBNL 25:75cce8f86272 97 {
ThomasBNL 25:75cce8f86272 98 motor2speed=output_motor; //then apply this value
ThomasBNL 25:75cce8f86272 99 motor2direction=1; //to the motor in anti-clockwise direction
ThomasBNL 25:75cce8f86272 100 break; // abort the loop and continue the script
ThomasBNL 25:75cce8f86272 101 }
ThomasBNL 25:75cce8f86272 102
ThomasBNL 25:75cce8f86272 103 //Case 2
ThomasBNL 25:75cce8f86272 104 while(output_motor<0) // Value to correct for by the motor <0
ThomasBNL 25:75cce8f86272 105 {
ThomasBNL 25:75cce8f86272 106 motor2speed=fabs(output_motor);// then apply the absolute value
ThomasBNL 25:75cce8f86272 107 motor2direction=0; // to the motor in clockwise direction
ThomasBNL 25:75cce8f86272 108 break; // then abort the loop and continue the script
ThomasBNL 25:75cce8f86272 109 }
ThomasBNL 25:75cce8f86272 110 // ATTACHING TICKER
ThomasBNL 25:75cce8f86272 111 {
ThomasBNL 25:75cce8f86272 112 TickerController.attach(&motor2_controller,0.01f);//100Hz
ThomasBNL 25:75cce8f86272 113 pc.printf("else loop controller");
ThomasBNL 25:75cce8f86272 114 }
ThomasBNL 26:6bdfc1856531 115 } // end of if-else loop 1
ThomasBNL 26:6bdfc1856531 116 } // end of while loop 1
ThomasBNL 26:6bdfc1856531 117 } // end of main
ThomasBNL 25:75cce8f86272 118
ThomasBNL 26:6bdfc1856531 119 //------------------------------------------------------------------------------------------------------------------------------------