James MacLean / Robot_Control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed-src pixylib

tuning.cpp@10:1acbcd54359b, 2016-03-25 (annotated)

Committer:
JamesMacLean
Date:
Fri Mar 25 20:40:29 2016 +0000
Revision:
10:1acbcd54359b
Parent:
8:b0478286ad21
Child:
13:d46282d84e47
My tuning program;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
balsamfir 2:2bc519e14bae 1 #include "tuning.h"
balsamfir 8:b0478286ad21 2 #include "robot.h"
balsamfir 2:2bc519e14bae 3
balsamfir 7:5ef312aa2678 4 void SystemTuning(void) {
balsamfir 8:b0478286ad21 5 RunMotor();
balsamfir 8:b0478286ad21 6 RunNavigation();
balsamfir 8:b0478286ad21 7 char key;
balsamfir 8:b0478286ad21 8 float increment = 0.0000001;
balsamfir 7:5ef312aa2678 9
balsamfir 8:b0478286ad21 10 while (1) {
balsamfir 8:b0478286ad21 11
balsamfir 8:b0478286ad21 12 ResetWatchDog();
balsamfir 8:b0478286ad21 13
balsamfir 8:b0478286ad21 14 if (bt.readable()) {
balsamfir 8:b0478286ad21 15 char key = bt.getc();
balsamfir 8:b0478286ad21 16 switch(key) {
balsamfir 8:b0478286ad21 17 case 'e':
balsamfir 8:b0478286ad21 18 motorKP = motorKP + increment;
balsamfir 8:b0478286ad21 19 break;
balsamfir 8:b0478286ad21 20 case 'e':
balsamfir 8:b0478286ad21 21 motorKP = motorKP + increment;
balsamfir 8:b0478286ad21 22 break;
balsamfir 8:b0478286ad21 23 case 'e':
balsamfir 8:b0478286ad21 24 motorKP = motorKP + increment;
balsamfir 8:b0478286ad21 25 break;
balsamfir 8:b0478286ad21 26 }
balsamfir 8:b0478286ad21 27 if (inKey == 'i') Kp = Kp + 0.000001;
balsamfir 8:b0478286ad21 28 else if (inKey == 'k') Kp = Kp - 0.000001;
balsamfir 8:b0478286ad21 29 else if (inKey == 'w') Ki = Ki + 0.000001;
balsamfir 8:b0478286ad21 30 else if (inKey == 's') Ki = Ki - 0.000001;
balsamfir 8:b0478286ad21 31
balsamfir 8:b0478286ad21 32 if (Setpoint == 6) Setpoint = 3;
balsamfir 8:b0478286ad21 33 else Setpoint = 6;
balsamfir 8:b0478286ad21 34
balsamfir 8:b0478286ad21 35 led4=0;
balsamfir 8:b0478286ad21 36 }
balsamfir 8:b0478286ad21 37
balsamfir 8:b0478286ad21 38 bt.printf("X Coordinate: %d, Height: %d \r\n", x, height);
balsamfir 8:b0478286ad21 39 bt.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering);
balsamfir 8:b0478286ad21 40 bt.printf("Left Motor Set: %f, Right Motor Set: %f \n\r", leftMotor, rightMotor);
balsamfir 8:b0478286ad21 41
balsamfir 8:b0478286ad21 42 Thread::wait(500); // Go to sleep for 500 ms
balsamfir 8:b0478286ad21 43 }
balsamfir 7:5ef312aa2678 44 }
balsamfir 7:5ef312aa2678 45
JamesMacLean 10:1acbcd54359b 46 //MotorTuning()
JamesMacLean 10:1acbcd54359b 47 //Tune the proportional gains until the robot goes straight
JamesMacLean 10:1acbcd54359b 48 // q = +speed
JamesMacLean 10:1acbcd54359b 49 // a = -speed
JamesMacLean 10:1acbcd54359b 50 // w = +kp left
JamesMacLean 10:1acbcd54359b 51 // S = -kp left
JamesMacLean 10:1acbcd54359b 52 // e = +kp right
JamesMacLean 10:1acbcd54359b 53 // d = -kp right
JamesMacLean 10:1acbcd54359b 54 // z = stop
JamesMacLean 10:1acbcd54359b 55 // x = start
JamesMacLean 10:1acbcd54359b 56 // p = print gain
JamesMacLean 10:1acbcd54359b 57 void MotorTuning(void) {
JamesMacLean 10:1acbcd54359b 58 float leftset=0,rightset=0;
JamesMacLean 10:1acbcd54359b 59 RunMotor();
JamesMacLean 10:1acbcd54359b 60
JamesMacLean 10:1acbcd54359b 61 pc.printf("q = +speed\r\n a = -speed\r\n w = +kp left\r\n s = -kp left\r\n e = +kp right\r\n d = -kp right\r\n z = stop\r\n x = start\r\n p = print gains\r\n");
JamesMacLean 10:1acbcd54359b 62
JamesMacLean 10:1acbcd54359b 63 while(1){
JamesMacLean 10:1acbcd54359b 64 ResetWatchDog();
JamesMacLean 10:1acbcd54359b 65
JamesMacLean 10:1acbcd54359b 66 if (bt.readable()) {
JamesMacLean 10:1acbcd54359b 67 char key = bt.getc();
JamesMacLean 10:1acbcd54359b 68 switch(key) {
JamesMacLean 10:1acbcd54359b 69 case 'x':
JamesMacLean 10:1acbcd54359b 70 rightset=5;
JamesMacLean 10:1acbcd54359b 71 leftset=5;
JamesMacLean 10:1acbcd54359b 72 break;
JamesMacLean 10:1acbcd54359b 73 case 'z':
JamesMacLean 10:1acbcd54359b 74 rightset=0;
JamesMacLean 10:1acbcd54359b 75 leftset=0;
JamesMacLean 10:1acbcd54359b 76 break;
JamesMacLean 10:1acbcd54359b 77 case 'q':
JamesMacLean 10:1acbcd54359b 78 rightset=rightset+1;
JamesMacLean 10:1acbcd54359b 79 leftset=leftset+1;
JamesMacLean 10:1acbcd54359b 80 break;
JamesMacLean 10:1acbcd54359b 81 case 'a':
JamesMacLean 10:1acbcd54359b 82 rightset=rightset+1;
JamesMacLean 10:1acbcd54359b 83 leftset=leftset+1;
JamesMacLean 10:1acbcd54359b 84 break;
JamesMacLean 10:1acbcd54359b 85 case 'w':
JamesMacLean 10:1acbcd54359b 86 leftMotorPI.kP = leftMotorPI.kP + 0.000001;
JamesMacLean 10:1acbcd54359b 87 break;
JamesMacLean 10:1acbcd54359b 88 case 's':
JamesMacLean 10:1acbcd54359b 89 leftMotorPI.kP = leftMotorPI.kP - 0.000001;
JamesMacLean 10:1acbcd54359b 90 break;
JamesMacLean 10:1acbcd54359b 91 case 'e':
JamesMacLean 10:1acbcd54359b 92 rightMotorPI.kP = rightMotorPI.kP + 0.000001;
JamesMacLean 10:1acbcd54359b 93 break;
JamesMacLean 10:1acbcd54359b 94 case 'd':
JamesMacLean 10:1acbcd54359b 95 rightMotorPI.kP = rightMotorPI.kP - 0.000001;
JamesMacLean 10:1acbcd54359b 96 break;
JamesMacLean 10:1acbcd54359b 97 case 'p':
JamesMacLean 10:1acbcd54359b 98 pc.printf("right gain: %f, left gain: %f\r\n",leftMotorPI.kP,rightMotorPI.kP);
JamesMacLean 10:1acbcd54359b 99 break;
JamesMacLean 10:1acbcd54359b 100 }
JamesMacLean 10:1acbcd54359b 101 }
JamesMacLean 10:1acbcd54359b 102
JamesMacLean 10:1acbcd54359b 103 osMutexWait(motorMutex, osWaitForever);
JamesMacLean 10:1acbcd54359b 104 leftMotor = leftSet;
JamesMacLean 10:1acbcd54359b 105 rightMotor = RightSet;
JamesMacLean 10:1acbcd54359b 106 osMutexRelease(motorMutex);
JamesMacLean 10:1acbcd54359b 107
JamesMacLean 10:1acbcd54359b 108 Thread::wait(500);
JamesMacLean 10:1acbcd54359b 109 }
balsamfir 2:2bc519e14bae 110 }
balsamfir 2:2bc519e14bae 111
balsamfir 2:2bc519e14bae 112 void SpeedTuning(void) {
balsamfir 2:2bc519e14bae 113
balsamfir 2:2bc519e14bae 114 }
balsamfir 2:2bc519e14bae 115
balsamfir 2:2bc519e14bae 116 void SteeringTuning(void) {
balsamfir 2:2bc519e14bae 117
balsamfir 2:2bc519e14bae 118 }
balsamfir 2:2bc519e14bae 119
balsamfir 2:2bc519e14bae 120 void TiltTuning(void) {
balsamfir 2:2bc519e14bae 121
balsamfir 2:2bc519e14bae 122 }