fatboyslim / Mbed 2 deprecated buttontest

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:   FRDM-TFC mbed

newAlgorithm.cpp@40:cdb079fc9d0a, 2015-03-09 (annotated)

Committer:
mperella
Date:
Mon Mar 09 19:27:22 2015 +0000
Revision:
40:cdb079fc9d0a
Child:
41:79ccb6f70ab6
added new file for new PID algorithm. haven't tried it out yet. gonna write a short thing in matlab to see what potential values we'd get

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mperella 40:cdb079fc9d0a 1 #include "TFC.h"
mperella 40:cdb079fc9d0a 2 #include <iostream>
mperella 40:cdb079fc9d0a 3 #include <stdio.h>
mperella 40:cdb079fc9d0a 4
mperella 40:cdb079fc9d0a 5 const float proportional = 0;
mperella 40:cdb079fc9d0a 6 const float last_proportional = 0;
mperella 40:cdb079fc9d0a 7 const float integral = 0;
mperella 40:cdb079fc9d0a 8 const float derivative = 0;
mperella 40:cdb079fc9d0a 9 const float output = 0;
mperella 40:cdb079fc9d0a 10
mperella 40:cdb079fc9d0a 11 const float kp = 0.05;
mperella 40:cdb079fc9d0a 12 const float ki = 0.02;
mperella 40:cdb079fc9d0a 13 const float kd = 0.02;
mperella 40:cdb079fc9d0a 14
mperella 40:cdb079fc9d0a 15 const int set_point = 63;
mperella 40:cdb079fc9d0a 16 const int position = 0;
mperella 40:cdb079fc9d0a 17 const int error = 0;
mperella 40:cdb079fc9d0a 18
mperella 40:cdb079fc9d0a 19 const float AGGRESSIVE = .60;
mperella 40:cdb079fc9d0a 20 const float MODERATE =.45;
mperella 40:cdb079fc9d0a 21 const float CONSERVATIVE =.30;
mperella 40:cdb079fc9d0a 22 const float STOP =0;
mperella 40:cdb079fc9d0a 23
mperella 40:cdb079fc9d0a 24 const int black_values_list[LINE_SCAN_LENGTH];
mperella 40:cdb079fc9d0a 25 const int black_value_count = 0;
mperella 40:cdb079fc9d0a 26 const int black_center_value = 0;
mperella 40:cdb079fc9d0a 27 const int sum_black = 0;
mperella 40:cdb079fc9d0a 28 const int violence_level = 0;
mperella 40:cdb079fc9d0a 29
mperella 40:cdb079fc9d0a 30 const float current_left_motor_speed = 0;
mperella 40:cdb079fc9d0a 31 const float current_right_motor_speed = 0;
mperella 40:cdb079fc9d0a 32
mperella 40:cdb079fc9d0a 33 const float current_right_max = 0;
mperella 40:cdb079fc9d0a 34 const float current_left_max = 0;
mperella 40:cdb079fc9d0a 35
mperella 40:cdb079fc9d0a 36
mperella 40:cdb079fc9d0a 37 int main()
mperella 40:cdb079fc9d0a 38 {
mperella 40:cdb079fc9d0a 39 TFC_Init();
mperella 40:cdb079fc9d0a 40 if(rear_motor_enable_flag)
mperella 40:cdb079fc9d0a 41 {
mperella 40:cdb079fc9d0a 42 TFC_HBRIDGE_ENABLE;
mperella 40:cdb079fc9d0a 43
mperella 40:cdb079fc9d0a 44 //current_left_motor_speed = (TFC_ReadPot(0));
mperella 40:cdb079fc9d0a 45 //current_right_motor_speed = (TFC_ReadPot(1));
mperella 40:cdb079fc9d0a 46
mperella 40:cdb079fc9d0a 47 // checking behavior level
mperella 40:cdb079fc9d0a 48 violence_level = int(TFC_GetDIP_Switch());
mperella 40:cdb079fc9d0a 49
mperella 40:cdb079fc9d0a 50 if (violence_level==3) {
mperella 40:cdb079fc9d0a 51 current_left_motor_speed = -(AGGRESSIVE);
mperella 40:cdb079fc9d0a 52 current_right_motor_speed = AGGRESSIVE;
mperella 40:cdb079fc9d0a 53 current_right_max = AGGRESSIVE;
mperella 40:cdb079fc9d0a 54 current_left_max = -AGGRESSIVE;
mperella 40:cdb079fc9d0a 55
mperella 40:cdb079fc9d0a 56 }
mperella 40:cdb079fc9d0a 57 else if (violence_level==2) {
mperella 40:cdb079fc9d0a 58 current_left_motor_speed = -(MODERATE);
mperella 40:cdb079fc9d0a 59 current_right_motor_speed = (MODERATE);
mperella 40:cdb079fc9d0a 60 current_right_max = MODERATE;
mperella 40:cdb079fc9d0a 61 current_left_max = -MODERATE;
mperella 40:cdb079fc9d0a 62 Kp = Kp * 0.75;
mperella 40:cdb079fc9d0a 63 Ki = Ki * 0.75;
mperella 40:cdb079fc9d0a 64 Kd = Kd * 0.75;
mperella 40:cdb079fc9d0a 65 }
mperella 40:cdb079fc9d0a 66 else if (violence_level==1) {
mperella 40:cdb079fc9d0a 67 current_left_motor_speed = -(CONSERVATIVE);
mperella 40:cdb079fc9d0a 68 current_right_motor_speed = CONSERVATIVE;
mperella 40:cdb079fc9d0a 69 current_right_max = CONSERVATIVE;
mperella 40:cdb079fc9d0a 70 current_left_max = CONSERVATIVE;
mperella 40:cdb079fc9d0a 71 Kp = Kp * 0.5;
mperella 40:cdb079fc9d0a 72 Ki = Ki * 0.5;
mperella 40:cdb079fc9d0a 73 Kd = Kd * 0.5;
mperella 40:cdb079fc9d0a 74 }
mperella 40:cdb079fc9d0a 75 else if (violence_level==0) {
mperella 40:cdb079fc9d0a 76 current_left_motor_speed = STOP;
mperella 40:cdb079fc9d0a 77 current_right_motor_speed = STOP;
mperella 40:cdb079fc9d0a 78 }
mperella 40:cdb079fc9d0a 79 else {
mperella 40:cdb079fc9d0a 80 current_left_motor_speed = STOP;
mperella 40:cdb079fc9d0a 81 current_right_motor_speed = STOP;
mperella 40:cdb079fc9d0a 82 }
mperella 40:cdb079fc9d0a 83
mperella 40:cdb079fc9d0a 84
mperella 40:cdb079fc9d0a 85 // protection block
mperella 40:cdb079fc9d0a 86 /*
mperella 40:cdb079fc9d0a 87 if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
mperella 40:cdb079fc9d0a 88 current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
mperella 40:cdb079fc9d0a 89 if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
mperella 40:cdb079fc9d0a 90 current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
mperella 40:cdb079fc9d0a 91 if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
mperella 40:cdb079fc9d0a 92 current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
mperella 40:cdb079fc9d0a 93 if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
mperella 40:cdb079fc9d0a 94 current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
mperella 40:cdb079fc9d0a 95 */
mperella 40:cdb079fc9d0a 96
mperella 40:cdb079fc9d0a 97 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 40:cdb079fc9d0a 98 }// end motor enabled
mperella 40:cdb079fc9d0a 99 else {
mperella 40:cdb079fc9d0a 100 TFC_HBRIDGE_DISABLE;
mperella 40:cdb079fc9d0a 101 }// end motor disabled
mperella 40:cdb079fc9d0a 102
mperella 40:cdb079fc9d0a 103 }
mperella 40:cdb079fc9d0a 104
mperella 40:cdb079fc9d0a 105 void loop()
mperella 40:cdb079fc9d0a 106 {
mperella 40:cdb079fc9d0a 107 if (linescan_enable)
mperella 40:cdb079fc9d0a 108 {
mperella 40:cdb079fc9d0a 109 if (TFC_LineScanImageReady !=0)
mperella 40:cdb079fc9d0a 110 {
mperella 40:cdb079fc9d0a 111 if (linescan_ping_pong)
mperella 40:cdb079fc9d0a 112 {
mperella 40:cdb079fc9d0a 113 position = getSensorValues();
mperella 40:cdb079fc9d0a 114 error = pid_calc(position);
mperella 40:cdb079fc9d0a 115 calcMotors(error);
mperella 40:cdb079fc9d0a 116 }
mperella 40:cdb079fc9d0a 117 }
mperella 40:cdb079fc9d0a 118 }
mperella 40:cdb079fc9d0a 119 else
mperella 40:cdb079fc9d0a 120 {
mperella 40:cdb079fc9d0a 121 TFC_SetMotorPWM(STOP, STOP);
mperella 40:cdb079fc9d0a 122 }
mperella 40:cdb079fc9d0a 123 }
mperella 40:cdb079fc9d0a 124
mperella 40:cdb079fc9d0a 125 int getSensorValues()
mperella 40:cdb079fc9d0a 126 {
mperella 40:cdb079fc9d0a 127 for (uint16_t i=0; i<128; i++)
mperella 40:cdb079fc9d0a 128 {
mperella 40:cdb079fc9d0a 129 if ((*(TFC_LineScanImage0+i) < 250))
mperella 40:cdb079fc9d0a 130 {
mperella 40:cdb079fc9d0a 131 black_values_list[black_value_count] = i;
mperella 40:cdb079fc9d0a 132 black_value_count++;
mperella 40:cdb079fc9d0a 133 }
mperella 40:cdb079fc9d0a 134 }
mperella 40:cdb079fc9d0a 135
mperella 40:cdb079fc9d0a 136 for(int i=0; i<black_value_count; i++)
mperella 40:cdb079fc9d0a 137 {
mperella 40:cdb079fc9d0a 138 sum_black += black_values_list[i];
mperella 40:cdb079fc9d0a 139 }
mperella 40:cdb079fc9d0a 140
mperella 40:cdb079fc9d0a 141 black_center_value = int(sum_black / black_value_count);
mperella 40:cdb079fc9d0a 142 return black_center_value;
mperella 40:cdb079fc9d0a 143 }
mperella 40:cdb079fc9d0a 144 }
mperella 40:cdb079fc9d0a 145
mperella 40:cdb079fc9d0a 146 int pid_calc(int pos)
mperella 40:cdb079fc9d0a 147 {
mperella 40:cdb079fc9d0a 148 position = pos;
mperella 40:cdb079fc9d0a 149 proportional = position - set_point;
mperella 40:cdb079fc9d0a 150 integral = integral + proportional;
mperella 40:cdb079fc9d0a 151 derivative= proportional - last_proportional;
mperella 40:cdb079fc9d0a 152 last_proportional = proportional;
mperella 40:cdb079fc9d0a 153 error = proportional*Kp + integral*Ki + derivative*Kd;
mperella 40:cdb079fc9d0a 154 }
mperella 40:cdb079fc9d0a 155
mperella 40:cdb079fc9d0a 156 void calcMotors(float error)
mperella 40:cdb079fc9d0a 157 {
mperella 40:cdb079fc9d0a 158 TFC_SetServo(0, error);
mperella 40:cdb079fc9d0a 159
mperella 40:cdb079fc9d0a 160 if(error < 0)
mperella 40:cdb079fc9d0a 161 {
mperella 40:cdb079fc9d0a 162 current_left_motor_speed = current_left_motor_speed + error; // reverse this
mperella 40:cdb079fc9d0a 163
mperella 40:cdb079fc9d0a 164 // make sure not going too fast
mperella 40:cdb079fc9d0a 165 if(current_right_motor_speed > current_right_max)
mperella 40:cdb079fc9d0a 166 {
mperella 40:cdb079fc9d0a 167 current_right_motor_speed = current_right_max;
mperella 40:cdb079fc9d0a 168 }
mperella 40:cdb079fc9d0a 169 else
mperella 40:cdb079fc9d0a 170 {
mperella 40:cdb079fc9d0a 171 current_right_motor_speed = current_right_motor_speed + error;
mperella 40:cdb079fc9d0a 172 }
mperella 40:cdb079fc9d0a 173 }
mperella 40:cdb079fc9d0a 174 else
mperella 40:cdb079fc9d0a 175 {
mperella 40:cdb079fc9d0a 176 current_right_motor_speed = current_right_motor_speed - error;
mperella 40:cdb079fc9d0a 177
mperella 40:cdb079fc9d0a 178 // make sure not going too fast
mperella 40:cdb079fc9d0a 179 if(current_left_motor_speed < current_left_max)
mperella 40:cdb079fc9d0a 180 {
mperella 40:cdb079fc9d0a 181 current_left_motor_speed = current_left_max;
mperella 40:cdb079fc9d0a 182 }
mperella 40:cdb079fc9d0a 183 else
mperella 40:cdb079fc9d0a 184 {
mperella 40:cdb079fc9d0a 185 current_left_motor_speed = current_left_motor_speed - error;
mperella 40:cdb079fc9d0a 186 }
mperella 40:cdb079fc9d0a 187 }
mperella 40:cdb079fc9d0a 188
mperella 40:cdb079fc9d0a 189 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 40:cdb079fc9d0a 190 }