jetfishteam / Mbed 2 deprecated brushless_test

unit test for brushless motors using a potentiometer and a castle creations esc with 0.5 center duty cycle

Dependencies:   ESC Servo mbed

Fork of brushlessmotor by jetfishteam

MainController.cpp@0:187bb46ed128, 2015-07-29 (annotated)

Committer:
demaille
Date:
Wed Jul 29 20:00:59 2015 +0000
Revision:
0:187bb46ed128
Child:
2:040b8c8f4f92
first brushless motor code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
demaille 0:187bb46ed128 1 #include "MainController.h"
demaille 0:187bb46ed128 2
demaille 0:187bb46ed128 3 MainController::MainController()
demaille 0:187bb46ed128 4 :ch1(p17,0.0636,0.1075), // yaw
demaille 0:187bb46ed128 5 ch2(p15,0.0623,0.1075), // pitch
demaille 0:187bb46ed128 6 ch3(p18,0.0614,0.1071), // amplitude
demaille 0:187bb46ed128 7 //ch4(p30,0.055,0.092), // adj
demaille 0:187bb46ed128 8 ch6(p16,0.0625,0.1055), // frequency
demaille 0:187bb46ed128 9 esc1(p21)
demaille 0:187bb46ed128 10 //ap(p25, p26)//,
demaille 0:187bb46ed128 11 //leftservo(p21),
demaille 0:187bb46ed128 12 //rightservo(p23)
demaille 0:187bb46ed128 13
demaille 0:187bb46ed128 14 {
demaille 0:187bb46ed128 15 wait_ms(50);
demaille 0:187bb46ed128 16 amp = 0.0;
demaille 0:187bb46ed128 17 frq = 1.0;
demaille 0:187bb46ed128 18 frqCmd = frq;
demaille 0:187bb46ed128 19 yaw = 0.5;
demaille 0:187bb46ed128 20 frqMin = 0.7; //hardcoded
demaille 0:187bb46ed128 21 frqMax = 1.8; //hardcoded
demaille 0:187bb46ed128 22 fullCycle = true;
demaille 0:187bb46ed128 23 raiser = 0.0;
demaille 0:187bb46ed128 24 alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
demaille 0:187bb46ed128 25
demaille 0:187bb46ed128 26 throttle_var = 0.5; //neutral position
demaille 0:187bb46ed128 27 esc1 = throttle_var;
demaille 0:187bb46ed128 28 esc1();
demaille 0:187bb46ed128 29 wait_ms(3000); //to arm brushless motor
demaille 0:187bb46ed128 30 //leftservo.calibrate(0.0008, 45);
demaille 0:187bb46ed128 31 //rightservo.calibrate(-0.0001, 45);
demaille 0:187bb46ed128 32
demaille 0:187bb46ed128 33 }
demaille 0:187bb46ed128 34
demaille 0:187bb46ed128 35 void MainController::control()
demaille 0:187bb46ed128 36 {
demaille 0:187bb46ed128 37 curTime = timer1.read();
demaille 0:187bb46ed128 38
demaille 0:187bb46ed128 39 // check every half cycle
demaille 0:187bb46ed128 40 if(curTime > 1/(2*frqCmd) ) {
demaille 0:187bb46ed128 41
demaille 0:187bb46ed128 42 // read new yaw value every half cycle
demaille 0:187bb46ed128 43 yaw = this->calculateYaw(); // a value from -1 to 1
demaille 0:187bb46ed128 44
demaille 0:187bb46ed128 45 if(yaw < 0.1 && yaw > -0.1){ // eliminating noise around 0.0 +-0.1
demaille 0:187bb46ed128 46 yaw =0.0;
demaille 0:187bb46ed128 47 }
demaille 0:187bb46ed128 48
demaille 0:187bb46ed128 49 // Read volume and frequency only every full cycle
demaille 0:187bb46ed128 50 if( fullCycle ) {
demaille 0:187bb46ed128 51 //read other new inputs only at upward portion of full cycle
demaille 0:187bb46ed128 52 amp = this->calculateAmplitude(); // a value from 0 to 1
demaille 0:187bb46ed128 53 frq = this->calculateFrequency(); // a value from frqmin to frqmax
demaille 0:187bb46ed128 54
demaille 0:187bb46ed128 55 ampNew = amp;
demaille 0:187bb46ed128 56
demaille 0:187bb46ed128 57 if(yaw < 0.0)
demaille 0:187bb46ed128 58 {
demaille 0:187bb46ed128 59 ampNew = (1.0+0.7*yaw)*amp;
demaille 0:187bb46ed128 60 }
demaille 0:187bb46ed128 61
demaille 0:187bb46ed128 62 fullCycle = false;
demaille 0:187bb46ed128 63
demaille 0:187bb46ed128 64 } else {
demaille 0:187bb46ed128 65 // reverse for the downward slope
demaille 0:187bb46ed128 66 amp = -amp;
demaille 0:187bb46ed128 67
demaille 0:187bb46ed128 68 ampNew = amp;
demaille 0:187bb46ed128 69
demaille 0:187bb46ed128 70 if(yaw > 0.0)
demaille 0:187bb46ed128 71 {
demaille 0:187bb46ed128 72 ampNew = (1.0-0.7*yaw)*amp;
demaille 0:187bb46ed128 73 }
demaille 0:187bb46ed128 74
demaille 0:187bb46ed128 75 // use amp and frq from last cycle in order to make sure it is equalized
demaille 0:187bb46ed128 76 fullCycle = true;
demaille 0:187bb46ed128 77 }
demaille 0:187bb46ed128 78 // update the frequency that actually needs to be commanded
demaille 0:187bb46ed128 79 frqCmd = frq;
demaille 0:187bb46ed128 80
demaille 0:187bb46ed128 81 // read new yaw value every half cycle
demaille 0:187bb46ed128 82 //adj = this->calculateAdj(); // a value from 0 to 1
demaille 0:187bb46ed128 83
demaille 0:187bb46ed128 84 // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller
demaille 0:187bb46ed128 85 //volume = volChg + volume;
demaille 0:187bb46ed128 86 // rudder value used to define the trapezoid shape
demaille 0:187bb46ed128 87 raiser = 5; // varied from 1 to 5
demaille 0:187bb46ed128 88
demaille 0:187bb46ed128 89 //reset timer
demaille 0:187bb46ed128 90 timer1.reset();
demaille 0:187bb46ed128 91 curTime = 0.0;
demaille 0:187bb46ed128 92 }
demaille 0:187bb46ed128 93
demaille 0:187bb46ed128 94
demaille 0:187bb46ed128 95 // //Set Servo Values
demaille 0:187bb46ed128 96 pitch = this->calculatePitch();
demaille 0:187bb46ed128 97 // leftservo = pitch+0.03;
demaille 0:187bb46ed128 98 // rightservo = 1.0 - pitch;
demaille 0:187bb46ed128 99 // if (rightservo<0.03){
demaille 0:187bb46ed128 100 // rightservo = 0.03;
demaille 0:187bb46ed128 101 // }
demaille 0:187bb46ed128 102
demaille 0:187bb46ed128 103 dutyCycle = 0.5 + ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
demaille 0:187bb46ed128 104 //set brushless motor speed
demaille 0:187bb46ed128 105 throttle_var = dutyCycle;
demaille 0:187bb46ed128 106 esc1 = throttle_var;
demaille 0:187bb46ed128 107 esc1();
demaille 0:187bb46ed128 108 }
demaille 0:187bb46ed128 109
demaille 0:187bb46ed128 110 float MainController::calculateFrequency()
demaille 0:187bb46ed128 111 {
demaille 0:187bb46ed128 112 return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin);
demaille 0:187bb46ed128 113 //return (ch6.dutycyclescaledup());
demaille 0:187bb46ed128 114 }
demaille 0:187bb46ed128 115
demaille 0:187bb46ed128 116 float MainController::calculateAmplitude()
demaille 0:187bb46ed128 117 {
demaille 0:187bb46ed128 118 return (ch3.dutycyclescaledup() / 2.0);
demaille 0:187bb46ed128 119 }
demaille 0:187bb46ed128 120
demaille 0:187bb46ed128 121 float MainController::calculateYaw()
demaille 0:187bb46ed128 122 {
demaille 0:187bb46ed128 123 return (2.0*ch1.dutycyclescaledup() - 1.0); //maps from 0.0-1.0 to -1.0 - 1.0
demaille 0:187bb46ed128 124 //return (ch1.dutycyclescaledup());
demaille 0:187bb46ed128 125 }
demaille 0:187bb46ed128 126
demaille 0:187bb46ed128 127 float MainController::calculatePitch()
demaille 0:187bb46ed128 128 {
demaille 0:187bb46ed128 129 //float pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup());
demaille 0:187bb46ed128 130 float pitAvg = (ch2.dutycyclescaledup());
demaille 0:187bb46ed128 131 return pitAvg;
demaille 0:187bb46ed128 132
demaille 0:187bb46ed128 133 }
demaille 0:187bb46ed128 134
demaille 0:187bb46ed128 135 //float MainController::calculateAdj()
demaille 0:187bb46ed128 136 //{
demaille 0:187bb46ed128 137 // return (ch4.dutycyclescaledup());
demaille 0:187bb46ed128 138 //}
demaille 0:187bb46ed128 139
demaille 0:187bb46ed128 140 void MainController::start()
demaille 0:187bb46ed128 141 {
demaille 0:187bb46ed128 142 timer1.start();
demaille 0:187bb46ed128 143
demaille 0:187bb46ed128 144 ticker1.attach(this, &MainController::control,0.001);
demaille 0:187bb46ed128 145 //Autopilot guardian
demaille 0:187bb46ed128 146 //ap.calibrate();
demaille 0:187bb46ed128 147 //ap.set2D();
demaille 0:187bb46ed128 148 //ap.setoff();
demaille 0:187bb46ed128 149
demaille 0:187bb46ed128 150 }
demaille 0:187bb46ed128 151
demaille 0:187bb46ed128 152 void MainController::stop()
demaille 0:187bb46ed128 153 {
demaille 0:187bb46ed128 154 timer1.stop();
demaille 0:187bb46ed128 155 ticker1.detach();
demaille 0:187bb46ed128 156 throttle_var = 0.5;
demaille 0:187bb46ed128 157 esc1 = throttle_var;
demaille 0:187bb46ed128 158 esc1();
demaille 0:187bb46ed128 159 }
demaille 0:187bb46ed128 160
demaille 0:187bb46ed128 161 float MainController::getDutyCycle()
demaille 0:187bb46ed128 162 {
demaille 0:187bb46ed128 163 return dutyCycle;
demaille 0:187bb46ed128 164 }
demaille 0:187bb46ed128 165
demaille 0:187bb46ed128 166 float MainController::getAmplitude()
demaille 0:187bb46ed128 167 {
demaille 0:187bb46ed128 168 return amp;
demaille 0:187bb46ed128 169 }
demaille 0:187bb46ed128 170
demaille 0:187bb46ed128 171
demaille 0:187bb46ed128 172 float MainController::getFrequency()
demaille 0:187bb46ed128 173 {
demaille 0:187bb46ed128 174 return frq;
demaille 0:187bb46ed128 175 }
demaille 0:187bb46ed128 176
demaille 0:187bb46ed128 177 float MainController::getYaw()
demaille 0:187bb46ed128 178 {
demaille 0:187bb46ed128 179 return yaw;
demaille 0:187bb46ed128 180 }
demaille 0:187bb46ed128 181
demaille 0:187bb46ed128 182 float MainController::getPitch()
demaille 0:187bb46ed128 183 {
demaille 0:187bb46ed128 184 return pitch;
demaille 0:187bb46ed128 185 }
demaille 0:187bb46ed128 186
demaille 0:187bb46ed128 187 //signum function
demaille 0:187bb46ed128 188 float MainController::signum(float input) //gives back the sign
demaille 0:187bb46ed128 189 {
demaille 0:187bb46ed128 190 if (input>0.0)
demaille 0:187bb46ed128 191 return 1.0;
demaille 0:187bb46ed128 192 else if (input<0.0)
demaille 0:187bb46ed128 193 return (-1.0);
demaille 0:187bb46ed128 194 else
demaille 0:187bb46ed128 195 return 0.0;
demaille 0:187bb46ed128 196 }
demaille 0:187bb46ed128 197
demaille 0:187bb46ed128 198 //saturate function
demaille 0:187bb46ed128 199 float MainController::saturate(float input) //saturates a value at 1.0 or -1.0
demaille 0:187bb46ed128 200 {
demaille 0:187bb46ed128 201 if (input > 1.0)
demaille 0:187bb46ed128 202 return (1.0);
demaille 0:187bb46ed128 203 else if (input < -1.0)
demaille 0:187bb46ed128 204 return (-1.0);
demaille 0:187bb46ed128 205 else
demaille 0:187bb46ed128 206 return input;
demaille 0:187bb46ed128 207 }