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@41:79ccb6f70ab6, 2015-03-10 (annotated)

Committer:
mperella
Date:
Tue Mar 10 13:51:08 2015 +0000
Revision:
41:79ccb6f70ab6
Parent:
40:cdb079fc9d0a
Child:
42:c189b914931d
added some comments

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