Interface Library for Madpulse RC Car
Dependents: Madpulse_Speed_Control_temp Madpulse_Speed_Control_Students
madpulse.cpp@1:6bba42f337e6, 2017-11-15 (annotated)
- Committer:
- jdawkins
- Date:
- Wed Nov 15 18:36:09 2017 +0000
- Revision:
- 1:6bba42f337e6
- Parent:
- 0:5e55308aa7a6
Commit for class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jdawkins | 0:5e55308aa7a6 | 1 | #include "mbed.h" |
jdawkins | 0:5e55308aa7a6 | 2 | #include "madpulse.h" |
jdawkins | 0:5e55308aa7a6 | 3 | #define DT 0.02 |
jdawkins | 0:5e55308aa7a6 | 4 | MadPulse::MadPulse(uint8_t id,Serial *port,BNO055 *imu, ServoIn *CH1,ServoIn *CH2, InterruptIn *irq, ServoOut *Steer, ServoOut *Throttle): |
jdawkins | 0:5e55308aa7a6 | 5 | id(id), |
jdawkins | 0:5e55308aa7a6 | 6 | _imu(imu), |
jdawkins | 0:5e55308aa7a6 | 7 | _port(port), |
jdawkins | 0:5e55308aa7a6 | 8 | _wheel_irq(irq), |
jdawkins | 0:5e55308aa7a6 | 9 | _CH1(CH1), |
jdawkins | 0:5e55308aa7a6 | 10 | _CH2(CH2), |
jdawkins | 0:5e55308aa7a6 | 11 | _Steer(Steer), |
jdawkins | 0:5e55308aa7a6 | 12 | _Throttle(Throttle) |
jdawkins | 0:5e55308aa7a6 | 13 | { |
jdawkins | 0:5e55308aa7a6 | 14 | ti.start(); //Start Local Timer |
jdawkins | 0:5e55308aa7a6 | 15 | |
jdawkins | 0:5e55308aa7a6 | 16 | _wheel_irq->rise(this, &MadPulse::wheelCallback); // Callback to Interrupt for Wheel Sensor |
jdawkins | 0:5e55308aa7a6 | 17 | |
jdawkins | 0:5e55308aa7a6 | 18 | if(_imu->check()) { |
jdawkins | 0:5e55308aa7a6 | 19 | |
jdawkins | 0:5e55308aa7a6 | 20 | printf("BNO055 connected\r\n"); |
jdawkins | 0:5e55308aa7a6 | 21 | _imu->setmode(OPERATION_MODE_CONFIG); |
jdawkins | 0:5e55308aa7a6 | 22 | _imu->SetExternalCrystal(1); |
jdawkins | 0:5e55308aa7a6 | 23 | _imu->setmode(OPERATION_MODE_NDOF); //Uses magnetometer |
jdawkins | 0:5e55308aa7a6 | 24 | _imu->set_angle_units(RADIANS); |
jdawkins | 0:5e55308aa7a6 | 25 | _imu->set_accel_units(MPERSPERS); |
jdawkins | 0:5e55308aa7a6 | 26 | _imu->set_anglerate_units(RAD_PER_SEC); |
jdawkins | 0:5e55308aa7a6 | 27 | _imu->setoutputformat(WINDOWS); |
jdawkins | 0:5e55308aa7a6 | 28 | _imu->set_mapping(2); |
jdawkins | 0:5e55308aa7a6 | 29 | |
jdawkins | 0:5e55308aa7a6 | 30 | // imu_LED = 1; // turns on IMU light when calibration is successful |
jdawkins | 0:5e55308aa7a6 | 31 | |
jdawkins | 0:5e55308aa7a6 | 32 | } |
jdawkins | 0:5e55308aa7a6 | 33 | // _port->attach(this,&MadPulse::rxCallback); |
jdawkins | 0:5e55308aa7a6 | 34 | |
jdawkins | 0:5e55308aa7a6 | 35 | }; |
jdawkins | 0:5e55308aa7a6 | 36 | void MadPulse::sampleData(){ |
jdawkins | 0:5e55308aa7a6 | 37 | getAccel(); |
jdawkins | 0:5e55308aa7a6 | 38 | getGyro(); |
jdawkins | 0:5e55308aa7a6 | 39 | getMag(); |
jdawkins | 0:5e55308aa7a6 | 40 | getEuler(); |
jdawkins | 0:5e55308aa7a6 | 41 | getSpeed(); |
jdawkins | 0:5e55308aa7a6 | 42 | |
jdawkins | 0:5e55308aa7a6 | 43 | //printf("Wheel Freq: %f\n\r",wheel_freq); |
jdawkins | 0:5e55308aa7a6 | 44 | |
jdawkins | 0:5e55308aa7a6 | 45 | } |
jdawkins | 0:5e55308aa7a6 | 46 | void MadPulse::getAccel(){ |
jdawkins | 0:5e55308aa7a6 | 47 | _imu->get_accel(); |
jdawkins | 0:5e55308aa7a6 | 48 | accel.x = _imu->accel.x; |
jdawkins | 0:5e55308aa7a6 | 49 | accel.y = _imu->accel.y; |
jdawkins | 0:5e55308aa7a6 | 50 | accel.z = _imu->accel.z; |
jdawkins | 0:5e55308aa7a6 | 51 | } |
jdawkins | 0:5e55308aa7a6 | 52 | void MadPulse::getGyro(){ |
jdawkins | 0:5e55308aa7a6 | 53 | _imu->get_gyro(); |
jdawkins | 0:5e55308aa7a6 | 54 | gyro.x = _imu->gyro.x; |
jdawkins | 0:5e55308aa7a6 | 55 | gyro.y = _imu->gyro.y; |
jdawkins | 0:5e55308aa7a6 | 56 | gyro.z = _imu->gyro.z; |
jdawkins | 0:5e55308aa7a6 | 57 | } |
jdawkins | 0:5e55308aa7a6 | 58 | void MadPulse::getMag(){ |
jdawkins | 0:5e55308aa7a6 | 59 | _imu->get_mag(); |
jdawkins | 0:5e55308aa7a6 | 60 | mag.x = _imu->mag.x; |
jdawkins | 0:5e55308aa7a6 | 61 | mag.y = _imu->mag.y; |
jdawkins | 0:5e55308aa7a6 | 62 | mag.z = _imu->mag.z; |
jdawkins | 0:5e55308aa7a6 | 63 | // printf("Mag:%.1f,%.1f,%.1f\n\r",_imu->mag.x,_imu->mag.y,_imu->mag.z); |
jdawkins | 0:5e55308aa7a6 | 64 | } |
jdawkins | 0:5e55308aa7a6 | 65 | void MadPulse::getEuler(){ |
jdawkins | 0:5e55308aa7a6 | 66 | _imu->get_angles(); |
jdawkins | 0:5e55308aa7a6 | 67 | euler.roll = _imu->euler.roll; |
jdawkins | 0:5e55308aa7a6 | 68 | euler.pitch = _imu->euler.pitch; |
jdawkins | 0:5e55308aa7a6 | 69 | euler.yaw = _imu->euler.yaw; |
jdawkins | 0:5e55308aa7a6 | 70 | } |
jdawkins | 0:5e55308aa7a6 | 71 | |
jdawkins | 0:5e55308aa7a6 | 72 | |
jdawkins | 0:5e55308aa7a6 | 73 | void MadPulse::getSpeed(){ |
jdawkins | 0:5e55308aa7a6 | 74 | |
jdawkins | 0:5e55308aa7a6 | 75 | float dt_loc = ti.read()-t_ws; |
jdawkins | 0:5e55308aa7a6 | 76 | |
jdawkins | 0:5e55308aa7a6 | 77 | |
jdawkins | 0:5e55308aa7a6 | 78 | if(dt_loc > 0.1){ |
jdawkins | 0:5e55308aa7a6 | 79 | wheel_freq = 0; |
jdawkins | 0:5e55308aa7a6 | 80 | } |
jdawkins | 0:5e55308aa7a6 | 81 | /* if( (dt < 0.05) && (dt > 0.002)) { |
jdawkins | 0:5e55308aa7a6 | 82 | // wheel_spd = (1/CTS_REV)/(dt_ws); // 0.036 is the wheel radius v = omega*r |
jdawkins | 0:5e55308aa7a6 | 83 | //wheel_freq = 1/dt_ws; |
jdawkins | 0:5e55308aa7a6 | 84 | } else { |
jdawkins | 0:5e55308aa7a6 | 85 | |
jdawkins | 0:5e55308aa7a6 | 86 | |
jdawkins | 0:5e55308aa7a6 | 87 | }*/ |
jdawkins | 0:5e55308aa7a6 | 88 | |
jdawkins | 0:5e55308aa7a6 | 89 | |
jdawkins | 0:5e55308aa7a6 | 90 | |
jdawkins | 0:5e55308aa7a6 | 91 | } |
jdawkins | 0:5e55308aa7a6 | 92 | |
jdawkins | 0:5e55308aa7a6 | 93 | void MadPulse::resetEnc(){ |
jdawkins | 0:5e55308aa7a6 | 94 | counts = 0; |
jdawkins | 0:5e55308aa7a6 | 95 | } |
jdawkins | 0:5e55308aa7a6 | 96 | void MadPulse::wheelCallback() |
jdawkins | 0:5e55308aa7a6 | 97 | { |
jdawkins | 0:5e55308aa7a6 | 98 | dt = ti.read()-t_ws; |
jdawkins | 0:5e55308aa7a6 | 99 | |
jdawkins | 0:5e55308aa7a6 | 100 | // if(dt < 0.0001){ |
jdawkins | 0:5e55308aa7a6 | 101 | if(dt < 0.0001){ |
jdawkins | 0:5e55308aa7a6 | 102 | wheel_freq = 1/dt_old; |
jdawkins | 0:5e55308aa7a6 | 103 | dt_old = dt_old; |
jdawkins | 0:5e55308aa7a6 | 104 | |
jdawkins | 0:5e55308aa7a6 | 105 | }else{ |
jdawkins | 0:5e55308aa7a6 | 106 | wheel_freq = 1/dt; |
jdawkins | 0:5e55308aa7a6 | 107 | dt_old = dt; |
jdawkins | 0:5e55308aa7a6 | 108 | } |
jdawkins | 0:5e55308aa7a6 | 109 | |
jdawkins | 0:5e55308aa7a6 | 110 | if(wheel_freq >200){ |
jdawkins | 0:5e55308aa7a6 | 111 | wheel_freq=wheel_freq_old; |
jdawkins | 0:5e55308aa7a6 | 112 | } |
jdawkins | 0:5e55308aa7a6 | 113 | wheel_freq_old = wheel_freq; |
jdawkins | 0:5e55308aa7a6 | 114 | /* if(dt > 0.1){ |
jdawkins | 0:5e55308aa7a6 | 115 | wheel_freq = 0; |
jdawkins | 0:5e55308aa7a6 | 116 | }*/ |
jdawkins | 0:5e55308aa7a6 | 117 | |
jdawkins | 0:5e55308aa7a6 | 118 | |
jdawkins | 0:5e55308aa7a6 | 119 | // Hall effect speed sensor callback |
jdawkins | 0:5e55308aa7a6 | 120 | wheel_LED = !wheel_LED; |
jdawkins | 0:5e55308aa7a6 | 121 | counts++; // Iterate tick counter |
jdawkins | 0:5e55308aa7a6 | 122 | dt_ws = ti.read()-t_ws; |
jdawkins | 0:5e55308aa7a6 | 123 | t_ws= ti.read(); |
jdawkins | 0:5e55308aa7a6 | 124 | } // end void he_callback |
jdawkins | 0:5e55308aa7a6 | 125 | |
jdawkins | 0:5e55308aa7a6 | 126 | void MadPulse::setSteer(float str){ |
jdawkins | 0:5e55308aa7a6 | 127 | str = saturateCmd(str); |
jdawkins | 0:5e55308aa7a6 | 128 | |
jdawkins | 0:5e55308aa7a6 | 129 | _Steer->write((int)((str*500.0)+1500.0)); // Write Steering Pulse |
jdawkins | 0:5e55308aa7a6 | 130 | } |
jdawkins | 0:5e55308aa7a6 | 131 | void MadPulse::setThrottle(float thr){ |
jdawkins | 0:5e55308aa7a6 | 132 | thr = saturateVal(thr,0.3,-0.3); |
jdawkins | 0:5e55308aa7a6 | 133 | _Throttle->write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse |
jdawkins | 0:5e55308aa7a6 | 134 | } |
jdawkins | 0:5e55308aa7a6 | 135 | |
jdawkins | 0:5e55308aa7a6 | 136 | |
jdawkins | 0:5e55308aa7a6 | 137 | uint8_t MadPulse::sendMessage(Serial *obj,char* msg,uint8_t len) |
jdawkins | 0:5e55308aa7a6 | 138 | { |
jdawkins | 0:5e55308aa7a6 | 139 | int i; |
jdawkins | 0:5e55308aa7a6 | 140 | for(i=0; i<len; i++) { |
jdawkins | 0:5e55308aa7a6 | 141 | obj->printf("%c",msg[i]); |
jdawkins | 0:5e55308aa7a6 | 142 | } |
jdawkins | 0:5e55308aa7a6 | 143 | obj->printf("\r\n"); |
jdawkins | 0:5e55308aa7a6 | 144 | return i; |
jdawkins | 0:5e55308aa7a6 | 145 | } |
jdawkins | 0:5e55308aa7a6 | 146 | |
jdawkins | 0:5e55308aa7a6 | 147 | void MadPulse::rxCallback(){ |
jdawkins | 0:5e55308aa7a6 | 148 | |
jdawkins | 0:5e55308aa7a6 | 149 | char c = _port->getc(); |
jdawkins | 0:5e55308aa7a6 | 150 | // pc.printf("%s",c); |
jdawkins | 0:5e55308aa7a6 | 151 | if(c == 'H' || c == 'h') { |
jdawkins | 0:5e55308aa7a6 | 152 | _port->printf("Command List...\r\n"); |
jdawkins | 0:5e55308aa7a6 | 153 | _port->printf("d - set open loop duty cycle\r\n"); |
jdawkins | 0:5e55308aa7a6 | 154 | _port->printf("r - run open loop control with current duty cycle\r\n"); |
jdawkins | 0:5e55308aa7a6 | 155 | _port->printf("h - display this prompt and exit\r\n"); |
jdawkins | 0:5e55308aa7a6 | 156 | _port->printf("t - set duration of test\r\n\n\n"); |
jdawkins | 0:5e55308aa7a6 | 157 | _port->printf("Enter Command\r\n"); |
jdawkins | 0:5e55308aa7a6 | 158 | } |
jdawkins | 0:5e55308aa7a6 | 159 | else if(c == 'R' || c == 'r') { |
jdawkins | 0:5e55308aa7a6 | 160 | // auto_flag = 1; |
jdawkins | 0:5e55308aa7a6 | 161 | auto_LED = 1; |
jdawkins | 0:5e55308aa7a6 | 162 | } |
jdawkins | 0:5e55308aa7a6 | 163 | else if(c == 'S' || c == 's') { |
jdawkins | 0:5e55308aa7a6 | 164 | // auto_flag = 0; |
jdawkins | 0:5e55308aa7a6 | 165 | auto_LED = 0; |
jdawkins | 0:5e55308aa7a6 | 166 | } |
jdawkins | 0:5e55308aa7a6 | 167 | else { |
jdawkins | 0:5e55308aa7a6 | 168 | |
jdawkins | 0:5e55308aa7a6 | 169 | } |
jdawkins | 0:5e55308aa7a6 | 170 | } |
jdawkins | 0:5e55308aa7a6 | 171 | |
jdawkins | 0:5e55308aa7a6 | 172 | |
jdawkins | 0:5e55308aa7a6 | 173 | int MadPulse::parseMessage(char * msg) |
jdawkins | 0:5e55308aa7a6 | 174 | { |
jdawkins | 0:5e55308aa7a6 | 175 | |
jdawkins | 0:5e55308aa7a6 | 176 | if(!strncmp(msg, "$POS", 4)) { |
jdawkins | 0:5e55308aa7a6 | 177 | // int arg1; |
jdawkins | 0:5e55308aa7a6 | 178 | float arg2,arg3,arg4; |
jdawkins | 0:5e55308aa7a6 | 179 | |
jdawkins | 0:5e55308aa7a6 | 180 | sscanf(msg,"$POS,%f,%f,%f\n",&arg2,&arg3,&arg4); |
jdawkins | 0:5e55308aa7a6 | 181 | |
jdawkins | 0:5e55308aa7a6 | 182 | pos.x = arg2; |
jdawkins | 0:5e55308aa7a6 | 183 | pos.y = arg3; |
jdawkins | 0:5e55308aa7a6 | 184 | pos.z = arg4; |
jdawkins | 0:5e55308aa7a6 | 185 | |
jdawkins | 0:5e55308aa7a6 | 186 | // pc.printf("idm: %d n:%f e:%f d:%f",arg1,pos_n,pos_e,pos_d); |
jdawkins | 0:5e55308aa7a6 | 187 | |
jdawkins | 0:5e55308aa7a6 | 188 | return 1; |
jdawkins | 0:5e55308aa7a6 | 189 | } //emd of $CMD |
jdawkins | 0:5e55308aa7a6 | 190 | |
jdawkins | 0:5e55308aa7a6 | 191 | if(!strncmp(msg, "$RUN", 4)) { |
jdawkins | 0:5e55308aa7a6 | 192 | |
jdawkins | 0:5e55308aa7a6 | 193 | return 1; |
jdawkins | 0:5e55308aa7a6 | 194 | } |
jdawkins | 0:5e55308aa7a6 | 195 | if(!strncmp(msg, "$STOP", 4)) { |
jdawkins | 0:5e55308aa7a6 | 196 | |
jdawkins | 0:5e55308aa7a6 | 197 | return 1; |
jdawkins | 0:5e55308aa7a6 | 198 | } |
jdawkins | 0:5e55308aa7a6 | 199 | |
jdawkins | 0:5e55308aa7a6 | 200 | |
jdawkins | 0:5e55308aa7a6 | 201 | return 0; |
jdawkins | 0:5e55308aa7a6 | 202 | |
jdawkins | 0:5e55308aa7a6 | 203 | } |